timor.Robot
Classes
An implementation of the robot base class, using the pinocchio library. |
|
Abstract base class for all robot classes |
Functions
|
The default cost function to evaluate the quality of an inverse kinematic solution. |
Module Contents
- class timor.Robot.PinRobot(*, wrapper, home_configuration=None, base_placement=None, rng=None)
An implementation of the robot base class, using the pinocchio library.
Pinocchio from stack of tasks (pinocchio-python) for modelling kinematics and dynamics.
PinRobot Constructor
- Parameters:
wrapper (pinocchio.RobotWrapper) – Pinocchio robot wrapper.
home_configuration (numpy.ndarray)
base_placement (timor.utilities.transformation.TransformationLike)
rng (Optional[numpy.random.Generator])
- __check_collisions(task, return_at_first, safety_margin=0)
Checks all collision pairs or all collisions pairs until the first colliding one is found.
- Parameters:
task (Task.Task) – The task to check for collisions
return_at_first (bool) – If true, this function behaves like a “has_collision”. If false, it behaves like a “get_all_collisions” function.
safety_margin (float) – The safety margin is a padding added to all obstacles to increase needed separation to be considered collision free. In the same units as the collision objects (default and suggested in meters).
- Returns:
Bool if return_at_first is true, otherwise a list of all colliding pairs.
- Return type:
Union[bool, List[Tuple[RobotBase, Union[RobotBase, timor.task.Obstacle.Obstacle]]]]
- abstractmethod __deepcopy__(memodict={})
Pin Robot cannot be deep-copied without losing the information on how bodies are connected.
- __getstate__()
Custom getstate (used for pickling) for pinocchio robot in order to enable multiprocessing.
Geometric data from pinocchio is no serializeable
- __setstate__(state)
Unpickle.
- __slots__ = ()
- _add_body(body, parent_joint, parent_frame_id)
Adds a single body to the robot. Note that collision and visual data are not updated!
- Parameters:
body (timor.Bodies.PinBody) – A body wrapper
parent_joint (Union[int, str]) – The index or name of the parent joint
parent_frame_id (int) – The id of the parent frame of the body (important to calculate body placement)
- Returns:
The index of the new body frame and indices for the newly added collision geometries
- Return type:
- _add_joint(joint, parent, bodies=(), fixed_joints=(), inertia=None, update_kinematics=True, update_home_collisions=True, **kwargs)
Adds a joint to the current robot model.
- Parameters:
joint (pinocchio.JointModel) – A pinocchio joint model instance
parent (Union[int, str]) – Either the index or the name of the parent joint to which the new one shall be appended
bodies (Union[timor.Bodies.PinBody, Collection[timor.Bodies.PinBody]]) – All bodies attached to the joint. To add N bodies, there need to be N-1 fixed joints.
fixed_joints (Union[Tuple[pinocchio.Frame, Ellipsis], List[pinocchio.Frame]]) – Fixed joints attached to the joint. Assumes bodies and fixed joints are added in the order b1-fj1-b2-fx2-…-bn
inertia (pinocchio.Inertia) – The inertia attached to the joint. If not provided, will be auto-calculated by the bodies added.
update_kinematics (bool) – If False, the datas will not be re-calculated, meaning the changes to not have an immediate effect on the results of kinematic/dynamic methods. Is useful when adding multiple joints, so just after the last change the data needs to be updated.
update_home_collisions (bool) – If False, the visual and collision geometries will not be updated, meaning this joint will neither be plotted nor considered for collision checking until done so. Useful for either building models that don’t need either of these features or when adding multiple joints, so this can be done after the last only.
kwargs –
All key-word-arguments supported by pin.Model.addJoint:
joint_placement: [SE3] -> Placement of the joint inside its parent joint.
joint_name: [str] -> If empty, name will be random
max_effort: [float] -> Maximal Joint Torque
max_velocity: [float] -> Maximal Joint Velocity
min_config and max_config: [float] -> Joint configuration limits
friction: [float] -> Joint friction parameters
damping: [float] -> Joint damping parameters
- Returns:
Index of the new joint
- Return type:
- _init_collision_pairs()
Adds all collision pairs to the robot.
- _known_ik_solvers
- _remove_home_collisions()
Removes all collision pairs that are in collision while the robot is in its current (usually home) configuration
This can be replaced by manually defining collision pairs to be checked or reading an SRDF file that defines the collision pairs that can be ignored. Keep in mind that this method leads to never detecting collisions that already exist in the current configuration of the robot.
- _resize_for_pinocchio(arr)
A helper function to make sure anything that can be expressed as (nDOF,) array is expressed as such.
Pinocchio can’t handle (nDOF, 1) arrays, so we need to squeeze them. :param arr: A numpy array. If this is None, this method will automatically return an array of zeros.
- Parameters:
arr (Optional[numpy.ndarray])
- _rng: numpy.random.Generator
- _tcp
- _update_collision_placement()
Updates the placement of collision geometry objects
- _update_geometry_placement()
Updates the collision (for collision detection) and visual (for plotting) geometry placements.
- _update_kinematic()
Can be called after changing the robot structure. This method updates the kinematic model of the robot
- _update_visual_placement()
Updates the placement of visual geometry objects
- accelerations: numpy.ndarray
- add_tcp_frame(parent_joint, placement)
Adds a Tool Center Point that will be used for inverse kinematic calculations.
- Parameters:
parent_joint (str) – The joint the TCP is attached to
placement (pinocchio.SE3) – Relative placement of the TCP in the parent joint frame
- Returns:
The index of the TCP frame
- collision: pinocchio.GeometryModel
- collision_data: pinocchio.GeometryData
- collision_info(task)
Calculates all collisions in the task and prints human-readable information.
- Parameters:
task (Task.Task) – Task to test for collisions
- property collision_objects: Tuple[hppfcl.hppfcl.CollisionObject]
Returns all collision objects of the robot (internally wrapped by pinocchio) as hppfcl collision objects.
- Returns:
A list of hppfcl collision objects that can be used for fast collision checking.
- Return type:
Tuple[hppfcl.hppfcl.CollisionObject]
- property collision_pairs: numpy.ndarray
Uses the collision_objects property and returns all pairs that are in collision
- Returns:
An nx2 numpy array of collision pair inidices, where n is the number of pairs in collision
- Return type:
- collisions(task, safety_margin=0)
Returns all collisions that appear between the robot and itself or obstacles in the task.
- Parameters:
- Returns:
A list of tuples (robot, obstacle) for robot an obstacle in collision
- Return type:
List[Tuple[RobotBase, Union[RobotBase, timor.task.Obstacle.Obstacle]]]
- configuration: numpy.ndarray
- data: pinocchio.Data
- property ddq: numpy.ndarray
Alias for self.accelerations
- Return type:
- property dq: numpy.ndarray
Alias for self.velocities
- Return type:
- fd(tau, q=None, dq=None, motor_inertia=False, friction=True)
Solves the forward dynamics assuming no external wrench (= force + torque).
By default considers friction and motor inertias. The inverse operation is the “id” method.
- Parameters:
tau (numpy.ndarray) – Torques commanded
q (numpy.ndarray) – Joint position; defaults to current position (self.configuration)
dq (numpy.ndarray) – Joint velocity; defaults to current velocity (self.velocities)
motor_inertia (bool) – Consider motor inertias
friction (bool) – Consider joint friction
- Returns:
ddq the resulting joint accelerations
- Return type:
- fk(configuration=None, kind='tcp', collision=False, visual=False)
Calculate the forward kinematics (joint state -> end-effector position).
- Parameters:
configuration (numpy.array) – Joint configuration the fk should be given for. If None, will take current configuration.
kind (str) – Either ‘tcp’, ‘joints’ or ‘full’.
collision (bool) – Perform updates on the collision geometry objects placements
visual (bool) – Perform updates on the visual geometry objects placements
- Returns:
np. array of either dim 4x4 (kind tcp) or numJointsx4x4 (kind joints) or numFramesx4x4 (kind full)
- friction(dq)
Computes the forces due to friction
- Parameters:
dq (numpy.ndarray)
- Return type:
- classmethod from_urdf(urdf_file, package_dir, **kwargs)
Utility wrapper to load information about a robot from URDF and build the according PinRobot.
- Parameters:
urdf_file (pathlib.Path)
package_dir (pathlib.Path)
- Return type:
- has_collisions(task, safety_margin=0)
Returns true if there is at least one collision in the task or one self-collision.
- has_self_collision(q=None)
Returns true if there are any self collisions in the current robot configuration.
If q is provided, the robot configuration will be changed. If not, this defaults to the current configuration.
- Parameters:
q (numpy.ndarray)
- Return type:
- id(q=None, dq=None, ddq=None, motor_inertia=True, friction=True, eef_wrench=None, eef_wrench_frame='world')
Solves inverse dynamics using recursive newton euler, assuming no external forces or torques.
Solves the equation M * a_q + b = tau_q, where a_q is the joint acceleration (ddq), M is the mass matrix, b is the drift (composed of Coriolis, ???, and gravitation) and tau_q is the applied torques in the joints.
- Parameters:
motor_inertia (bool) – Whether to consider torques to balance motor side inertia. Even if set to true, the coriolis forces induced by the fast-rotating motors are not considered.
friction (bool) – Whether to consider friction compensating torques
eef_wrench (numpy.ndarray) – [f_x, f_y, f_z, tau_x, tau_y, tau_z] acting on the end-effector
eef_wrench_frame (str) – Frame the eef_wrench is described in (still acting on eef position!); for now only “world” accepted. TODO : Extend with future frame definitions
q (numpy.ndarray)
dq (numpy.ndarray)
ddq (numpy.ndarray)
- Return type:
- ik(eef_pose, *, ik_method=None, **kwargs)
Interface for inverse kinematics solver.
- Parameters:
eef_pose (timor.utilities.tolerated_pose.ToleratedPose) – The desired 4x4 placement of the end effector
ik_method (Optional[str]) – The method to use for the inner loop. Defaults to jacobian for robots with >=6 dof and scipy for robots with <6 dof.
- Returns:
A tuple of (q_solution, success [boolean])
- Return type:
Tuple[numpy.ndarray, bool]
- ik_jacobian(eef_pose, info, q_init, callbacks=(), damp=1e-12, ik_cost_function=default_ik_cost_function, convergence_threshold=1e-08, alpha_average=0.5, error_term_mask=None, gain=0.5, kind='damped_ls_inverse', tcp_or_world_frame='tcp')
Implements numerics inverse kinematics solvers based on the jacobian.
This method runs a single numerical IK until it converges or fails due to constraints or the maximum number of iterations. As a user, usually you want to try multiple random restarts and use the generalized ik interface that still allows you to choose the jacobian method for the inner loop.
If no solution is found, returns success=False and the configuration q with the lowest euclidean distance to the desired pose (ignoring orientation offset) that was found during max_iter iterations.
Reference: Robotics, Siciliano et al. [2009], Chapter 3.7 Reference: https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/md_doc_b-examples_i-inverse-kinematics.html # noqa: E501 Reference: https://scaron.info/robotics/inverse-kinematics.html
- Parameters:
eef_pose (timor.utilities.tolerated_pose.ToleratedPose) – Desired end effector pose with tolerances.
info (timor.utilities.dtypes.IKMeta) – An IKMeta object that contains relevant meta information about the IK problem and can be adapted during the inner IK process.
q_init (numpy.ndarray) – The joint configuration to start with.
callbacks (Iterable[timor.utilities.callbacks.IKCallback]) – An iterable of callbacks that are called after each iteration during optimization.
damp (float) – Damping for the damped least squares pseudoinverse method
ik_cost_function (Callable[[RobotBase, numpy.ndarray, timor.utilities.transformation.TransformationLike], float]) – While this cost function is not directly minimized, it is used to determine the quality of intermediate solutions and initial guesses if q_init is not provided.
convergence_threshold (float) – Threshold for termination. Optimization terminates when the exponentially decaying average of the cost function improvements is below this threshold.
alpha_average (float) – This factor controls the exponential decay of the running average computation, needed for the “convergence_threshold” stopping criterion. It must be between 0 and 1, where alpha=1 means no averaging at all and alpha close to 0 means averaging over many iterations. (0=average over all iterations)
error_term_mask (Optional[numpy.ndarray]) – A (6,) boolean mask that determines which components of the error term are considered. Depending on tcp_or_world_frame, the error term is either a [v, w] twist in the tcp frame or a [x, y, z, alpha, beta, gamma] vector in the world, where alpha, beta, gamma are the axis angles of the rotation. For example, masking the last three error terms [True, True, True, False, False, False] results in a partial inverse kinematics problem that only considers the position of the tcp.
gain (Union[float, numpy.ndarray]) – Gain Matrix K determining the step size per joint \(K J^{-q} e\). If a scalar is provided, it is interpreted as the same gain for all joints.
kind – One of “transpose”, “pseudo_inverse”, “damped_ls_inverse”. The according variant of the Jacobian will be used to solve the ik problem. While the transpose is the fastest one to calculate, for complicated problems it is too inaccurate to converge (quickly) to a solution. Pseudo inverse and damped least squares pseudo-inverse are pretty similar, the latter one introduces a “punishment” for the norm of the joint velocities and is therefore more resistant against singularities.
tcp_or_world_frame (str) – Determines in which frame the Jacobian and the error term are computed. While tcp frame should be preferred for most applications, world frame allows for solving partial inverse kinematics for under-determined problems. See “error_term_mask” for more details.
- Returns:
A tuple of q_solution [np.ndarray], success [boolean]. If success is False, q_solution is the configuration with minimal IK cost amongst all seen in between iterations of this method. The IK is successful if the tcp pose satisfies the tolerances of eef_pose and all callbacks return True.
- Return type:
Tuple[numpy.ndarray, bool]
- ik_jacobian_cpp(eef_pose, info, q_init, callbacks=(), damp=1e-12, ik_cost_function=default_ik_cost_function, convergence_threshold=1e-08, alpha_average=0.5, step_size=0.1, eps=0.0001, check_self_collisions=True, allow_inner_restart=True, weight_translation=1.0, weight_rotation=0.5 / np.pi)
Interface for C++ inverse kinematics solver.
- Parameters:
eef_pose (timor.utilities.tolerated_pose.ToleratedPose) – Desired end effector pose with tolerances.
info (timor.utilities.dtypes.IKMeta) – An IKMeta object that contains relevant meta information about the IK problem and can be adapted during the inner IK process (here mainly max_iter).
q_init (numpy.ndarray) – The joint configuration to start with.
callbacks (Iterable[timor.utilities.callbacks.IKCallback]) – An iterable of callbacks that are called after each iteration during optimization.
damp (float) – Damping for the damped least squares pseudoinverse method.
ik_cost_function (Callable[[RobotBase, numpy.ndarray, timor.utilities.transformation.TransformationLike], float]) – While this cost function is not directly minimized, it is used to determine the quality of intermediate solutions and initial guesses if q_init is not provided.
convergence_threshold (float) – Threshold for termination. Optimization terminates when the exponentially decaying average of the cost function improvements is below this threshold.
alpha_average (float) – This factor controls the exponential decay of the running average computation of cost.
step_size (float) – Step size for the optimization.
eps (float) – Epsilon for the optimization, i.e., if distance between log maps of desired and found eef and tcp is bellow this value the C++ side will return.
check_self_collisions (bool) – If True, self collisions are checked during the optimization and solutions that self-collide are invalid.
allow_inner_restart (bool) – If True, the C++ side is allowed to restart the optimization with a random guess, e.g., to avoid joint limits, self-collisions, or local minima.
weight_translation (float) – Weight for the translation part (L2 distance in meters) of the cost function.
weight_rotation (float) – Weight for the rotation part (angular distance in rad) of the cost function.
- Returns:
A tuple of q_solution [np.ndarray], success [boolean]. If success is False, q_solution is closest found guess according to the cost function.
- Return type:
Tuple[numpy.ndarray, bool]
- ik_scipy(eef_pose, info, q_init=None, callbacks=(), ik_cost_function=default_ik_cost_function, convergence_threshold=1e-08, scipy_kwargs=None)
Computes the inverse kinematics (joint angles for a given goal destination).
This method runs a single numerical IK until it converges or fails due to constraints or the maximum number of iterations. As a user, usually you want to try multiple random restarts and use the generalized ik interface that still allows you to choose the scipy minimize method for the inner loop.
- Parameters:
eef_pose (timor.utilities.tolerated_pose.ToleratedPose) – Desired end effector pose with tolerances.
info (timor.utilities.dtypes.IKMeta) – An IKMeta object that contains relevant meta information about the IK problem and can be adapted during the inner IK process.
q_init (Optional[numpy.ndarray]) – Initial joint angles to start searching from. If None, uses a random configuration
callbacks (Iterable[timor.utilities.callbacks.IKCallback]) – An iterable of callbacks that are called after each iteration during optimization.
ik_cost_function (Callable[[RobotBase, numpy.ndarray, timor.utilities.transformation.TransformationLike], float]) – A custom cost function that takes a robot, its joint angles, and a TransformationLike and returns a scalar cost indicating the quality of the configuration in solving the ik problem. If not given, the default cost function is used (equal weighting of .5 meter / 180 degree error).
convergence_threshold (float) – Threshold for termination. Optimization terminates when two consecutive iterates are improving the cost by less than this.
scipy_kwargs (Optional[Dict]) – Any keyword arguments that should be passed to the scipy minimize method. If not given, the ‘L-BFGS-B’ method with defaults will be used.
- Returns:
A tuple of q_solution [np.ndarray], success [boolean]. If success is False, q_solution is the configuration with minimal IK cost amongst all seen in between iterations of this method. The IK is successful if the tcp pose satisfies the tolerances of eef_pose and all callbacks return True.
- Return type:
Tuple[numpy.ndarray, bool]
- property joint_acceleration_limits: numpy.ndarray
Numpy array where arr[0,:] are the lower joint limits and arr[1,:] are the upper acceleration limits.
- Return type:
- property joint_limits: numpy.ndarray
Numpy array where arr[0,:] are the lower joint limits and arr[1,:] are the upper joint limits.
- Return type:
- property joint_torque_limits: numpy.ndarray
Numpy array where arr[0,:] are the lower joint limits and arr[1,:] are the upper torque limits.
- Return type:
- property joint_velocity_limits: numpy.ndarray
Numpy array with upper bound absolute joint velocity limits.
- Return type:
- property joints: List[str]
Returns the names of the “real” joints in the robot. (ignoring universe, which is, in pinocchio, a joint)
- Return type:
List[str]
- manipulability_index(q=None)
Calculate the Yoshikawa manipulability index at joint position q or current one.
- Parameters:
q (Optional[numpy.ndarray]) – Joint position to calculate at (default current robot configuration)
- Return type:
- property mass: float
Robot mass in kg, included the unactuated mass belonging to the base.
Pinocchio calculates the robot mass as the actuated mass only, omitting the mass attached to the “universe”. Opposed to this approach, we include the mass of the (static) base in the robot mass. :source: https://github.com/stack-of-tasks/pinocchio/issues/1286
- Return type:
- model: pinocchio.Model
- motor_inertias(ddq)
Computes the motor inertias in the joins at a given acceleration
- Parameters:
ddq (numpy.ndarray)
- Return type:
- move(displacement)
Moves frames, joints and geometries connected to the world.
- Parameters:
displacement (timor.utilities.transformation.TransformationLike) – Transformation / transformation transform to move, given in the world/universe frame
- property parents: Dict[str, str]
Returns a mapping from a joint to its parent. Assumes there is a unique parent for each joint!
- property placement: timor.utilities.transformation.Transformation
The robot’s base placement. Wrapped as property so it can only be set using the according methods.
- Return type:
- property q: numpy.ndarray
Alias for self.configuration
- Return type:
- q_in_joint_limits(q)
Returns true if a configuration q is within the joint limits of the robot
- Parameters:
q (numpy.ndarray)
- Return type:
- random_configuration(rng=None)
Generates a random configuration.
- Parameters:
rng (Optional[numpy.random.Generator]) – Random number generator to use. If not given, this method uses the pinocchio default.
- Return type:
- set_base_placement(placement)
Moves the base to desired position
- Parameters:
placement (timor.utilities.transformation.TransformationLike)
- Return type:
None
- static_torque(q, f_ext=None)
Static torques of the robot
- Parameters:
q (numpy.ndarray) – Current joint configuration
f_ext (pinocchio.StdVec_Force) – External forces as array of pin forces
- Returns:
n_joints x 1 array of torques
- Return type:
- tau_in_torque_limits(tau)
Returns true if the torque vector tau is within the torque limits of the robot.
We assume that the torque limits of the robot are symmetric (tau_min = -tau_max)
- Parameters:
tau (numpy.ndarray) – A torque vector of dimension dof x 1. The limits will be checked against abs(tau)
- Return type:
- property tcp
The index of the frame indicating the robot’s TCP (Tool Center Point).
- property tcp_acceleration: numpy.ndarray
Returns the current tcp velocity as 6x1 numpy array - relative to the world coordinate system.
- Returns:
Array (ddx, ddy, ddz, dd_alpha, dd_beta, dd_gamma) / dt
- Return type:
- property tcp_velocity: numpy.ndarray
Returns the current tcp velocity as 6x1 numpy array - per default relative to the world coordinate system.
- Parameters:
reference_frame – Select reference frame to represent tcp velocity in; details see: https://docs.ros.org/en/kinetic/api/pinocchio/html/group__pinocchio__multibody.html
- Returns:
Array math:(Delta x, Delta y, Delta z, Delta alpha, Delta beta, Delta gamma) / Delta t
- Return type:
- update_configuration(q, dq=None, ddq=None, *, frames=False, geometry=False)
Update the robot configuration by updating the joint parameters.
- Parameters:
q (numpy.ndarray) – The new configuration in joint space iven as numpy array of joint values.
dq (numpy.ndarray) – Joint velocities, optional
ddq (numpy.ndarray) – Joint acceleration, optional
frames (bool) – If true, frame placements are also updated
geometry (bool) – If true, geometry (visual and collision) placements are updated
- Returns:
None
- velocities: numpy.ndarray
- visual: pinocchio.GeometryModel
- visual_data: pinocchio.GeometryData
- visualize(visualizer=None, coordinate_systems=None, update_collision_visuals=True)
Displays the robot in its current environment using a Meshcat Visualizer in the browser
- Parameters:
visualizer (pinocchio.visualize.BaseVisualizer) – A meshcat visualizer instance. If none, a new will be created
coordinate_systems (Union[str, None]) – Can be None, ‘tcp’, ‘joints’ or ‘full’ - the specified coordinates will be drawn (full means all frames, as in fk).
update_collision_visuals (Optional[bool]) – If true, the collision geometry will be updated as well (but not shown by default).
- Returns:
The meshcat visualizer instance used for plotting the robot
- Return type:
pinocchio.visualize.MeshcatVisualizer
- visualize_self_collisions(visualizer=None)
Computes all collisions and visualizes them in a different color for every collision pair in contact.
- Note:
Only considers the pre-defined collision pairs.
- Parameters:
visualizer (pinocchio.visualize.MeshcatVisualizer) – A pinnocchio meshcat visualizer (only tested for this one)
- Return type:
pinocchio.visualize.MeshcatVisualizer
- class timor.Robot.RobotBase(*, name, home_configuration=None, base_placement=None, rng=None)
Abstract base class for all robot classes
RobotBase Constructor
- Parameters:
home_configuration (numpy.ndarray) – Home configuration of the robot (default pin.neutral)
base_placement (Optional[timor.utilities.transformation.TransformationLike]) – Home configuration of the robot (default pin.neutral)
rng (Optional[numpy.random.Generator]) – Optionally, a numpy random number generator to use for anything randomized happening with this robot can be provided. This rng will be the default, but can be overridden by users explicitly desiring to use another one at times.
name (str)
- __slots__ = ()
- _known_ik_solvers
- _rng: numpy.random.Generator
- accelerations: numpy.ndarray
- collision_info(task)
Calculates all collisions in the task and prints human-readable information.
- Parameters:
task (Task.Task) – Task to test for collisions
- abstractmethod collisions(task, safety_margin=0)
Returns all collisions that appear between the robot and itself or obstacles in the task.
- Parameters:
- Returns:
A list of tuples (robot, obstacle) for robot an obstacle in collision
- Return type:
List[Tuple[RobotBase, timor.task.Obstacle.Obstacle]]
- configuration: numpy.ndarray
- property ddq: numpy.ndarray
Alias for self.accelerations
- Return type:
- property dq: numpy.ndarray
Alias for self.velocities
- Return type:
- abstractmethod fk(configuration=None, kind='tcp')
Returns the forward kinematics for the robot as homogeneous transformation.
- Parameters:
configuration (numpy.array) – Joint configuration the fk should be given for. If None, will take current configuration.
kind (str) – Depends on the class, but should at least support ‘tcp’ and ‘joints’.
- Return type:
Union[timor.utilities.transformation.Transformation, Tuple[timor.utilities.transformation.Transformation]]
- classmethod from_urdf(urdf_file, package_dir, **kwargs)
- Abstractmethod:
- Parameters:
urdf_file (pathlib.Path)
package_dir (pathlib.Path)
- Return type:
Class constructor from urdf file. kwargs should at least handle RobotBase keyword arguments.
- abstractmethod has_collisions(task, safety_margin=0)
Returns true if there is at least one collision in the task or one self-collision.
Faster than self.collisions in case of collision, as this method breaks as soon as the first one is detected.
- abstractmethod has_self_collision(q=None)
Returns true if there are any self collisions in configuration q (defaults to current configuration)
- Parameters:
q (numpy.ndarray)
- Return type:
- abstractmethod id(q=None, dq=None, ddq=None, **kwargs)
Inverse Dynamics solver, expected to return torques as 1xnJoints array.
Should work with current configuration if not explicitly given q, dq and ddq.
- Parameters:
q (numpy.ndarray)
dq (numpy.ndarray)
ddq (numpy.ndarray)
- Return type:
- ik(eef_pose, *, allow_random_restart=True, current_best=None, ignore_self_collisions=False, ik_cost_function=default_ik_cost_function, ik_method='scipy', inner_callbacks=None, joint_mask=None, max_iter=1000, outer_callbacks=None, outer_constraints=None, q_init=None, task=None, convergence_threshold=1e-08, **kwargs)
Try to find the joint angles q that minimize the error between TCP pose and the desired eef_pose.
This serves as the general interface to ik methods. In general, these methods can be split in an inner and an outer loop. The inner loop (that could, theoretically also be a single step only) is responsible for (mostly numeric) optimization, while the outer loop is responsible for restarting the inner loop if it fails to converge or satisfy some constraints. This method is the outer loop, while the inner loop is determined by the ik_method argument.
- Parameters:
eef_pose (timor.utilities.tolerated_pose.ToleratedPose) – The desired 4x4 pose of the end effector.
allow_random_restart (bool) – If True, the IK will restart from random configurations if it fails to converge. Disable this for smooth motions (so, if you are not interested in solutions far away from q_init).
current_best (Optional[timor.utilities.dtypes.IntermediateIkResult]) – The current best solution, if any. In case of failure, the best intermediate solution will be returned
ignore_self_collisions (bool) – If True, self collisions will be ignored during the IK. By default, a solution with self collisions will never be considered successful.
inner_callbacks (Optional[Iterable[timor.utilities.callbacks.IKCallback]]) – Any number of callbacks that will be called after each iteration of the inner loop.
ik_cost_function (Callable[[RobotBase, numpy.ndarray, timor.utilities.transformation.TransformationLike], float]) – A custom cost function that takes a robot, its joint angles, and a TransformationLike and returns a scalar cost indicating the quality of the configuration in solving the ik problem.
ik_method (str) – The method to use for the inner loop. Defaults to ‘scipy’.
joint_mask (Optional[Sequence[bool]]) – A boolean array that indicates which joints should be considered for the IK. If an index evaluates to False, the joint will not be moved.
max_iter (int) – The maximum number of iterations before the algorithm gives up.
outer_callbacks (Optional[Iterable[timor.utilities.callbacks.IKCallback]]) – Any number of callbacks that will be called after each iteration of the outer loop.
outer_constraints (Optional[Iterable[Constraints.RobotConstraint]]) – Any number of constraints that must be satisfied by the solution but can not be incorporated in the inner optimization. The constraints are validated using the task provided to the ik and a joint velocity of zero. Joint limits and self collisions will always be considered if not explicitly ignored. The constraints are internally converted to callbacks that return false if the constraint is hurt.
task (Optional[Task.Task]) – If a task is provided, any of the constraints defined in the task will be considered outer constraints.
convergence_threshold (float) – A threshold for convergence: When the cost function stops improving by more than this threshold, the IK will terminate even without finding a solution.
q_init (Optional[numpy.ndarray]) – The joint configuration to start with. If not given, it is up to the downstream method to determine a suitable initial configuration.
kwargs – Any keyword arguments that are specific to the inner loop method.
- Returns:
A tuple of (q_solution, success [boolean])
- Note:
To debug the IK look into IKDebug dataclass defined in dtypes; it provides callbacks for logging all (sub)steps and analyzing each IK solution attempt.
- Return type:
Tuple[numpy.ndarray, bool]
- ik_scipy(eef_pose, info, q_init=None, callbacks=(), ik_cost_function=default_ik_cost_function, convergence_threshold=1e-08, scipy_kwargs=None)
Computes the inverse kinematics (joint angles for a given goal destination).
This method runs a single numerical IK until it converges or fails due to constraints or the maximum number of iterations. As a user, usually you want to try multiple random restarts and use the generalized ik interface that still allows you to choose the scipy minimize method for the inner loop.
- Parameters:
eef_pose (timor.utilities.tolerated_pose.ToleratedPose) – Desired end effector pose with tolerances.
info (timor.utilities.dtypes.IKMeta) – An IKMeta object that contains relevant meta information about the IK problem and can be adapted during the inner IK process.
q_init (Optional[numpy.ndarray]) – Initial joint angles to start searching from. If None, uses a random configuration
callbacks (Iterable[timor.utilities.callbacks.IKCallback]) – An iterable of callbacks that are called after each iteration during optimization.
ik_cost_function (Callable[[RobotBase, numpy.ndarray, timor.utilities.transformation.TransformationLike], float]) – A custom cost function that takes a robot, its joint angles, and a TransformationLike and returns a scalar cost indicating the quality of the configuration in solving the ik problem. If not given, the default cost function is used (equal weighting of .5 meter / 180 degree error).
convergence_threshold (float) – Threshold for termination. Optimization terminates when two consecutive iterates are improving the cost by less than this.
scipy_kwargs (Optional[Dict]) – Any keyword arguments that should be passed to the scipy minimize method. If not given, the ‘L-BFGS-B’ method with defaults will be used.
- Returns:
A tuple of q_solution [np.ndarray], success [boolean]. If success is False, q_solution is the configuration with minimal IK cost amongst all seen in between iterations of this method. The IK is successful if the tcp pose satisfies the tolerances of eef_pose and all callbacks return True.
- Return type:
Tuple[numpy.ndarray, bool]
- property joint_acceleration_limits: numpy.ndarray
- Abstractmethod:
- Return type:
Returns an array of shape 1xdof containing the absolute joint acceleration limits
- property joint_limits: numpy.ndarray
- Abstractmethod:
- Return type:
Returns an array of shape 2xdof containing the lower and upper joint position limits
- property joint_torque_limits: numpy.ndarray
- Abstractmethod:
- Return type:
Returns an array of shape 1xdof containing the absolute joint torque limits
- property joint_velocity_limits: numpy.ndarray
- Abstractmethod:
- Return type:
Returns an array of shape 1xdof containing the absolute joint velocity limits
- property joints: List[str]
- Abstractmethod:
- Return type:
List[str]
Returns a list containing all joint IDs of the robot
- abstractmethod manipulability_index(q)
Calculate the Yoshikawa manipulability index at joint position q or current one.
- Parameters:
q (Optional[numpy.ndarray]) – Joint position to calculate at (default current robot configuration)
- Return type:
- abstractmethod move(displacement)
Moves the whole robot by displacement
- Parameters:
displacement (timor.utilities.transformation.TransformationLike) – A 4x4 Transformation
- Return type:
None
- property parents: Dict[str, str]
-
Returns a mapping between joints and their parents. The base/world joint is its own parent
- property placement: timor.utilities.transformation.Transformation
- Abstractmethod:
- Return type:
The (base) placement in the world coordinate system (4x4 homogeneous transformation / placement)
- property q: numpy.ndarray
Alias for self.configuration
- Return type:
- q_in_joint_limits(q)
Returns true if a configuration q is within the joint limits of the robot
- Parameters:
q (numpy.ndarray)
- Return type:
- random_configuration(rng=None)
Generates a random configuration.
- Parameters:
rng (Optional[numpy.random.Generator]) – Random number generator to use. If not given, this method uses the robot default rng.
- Return type:
- abstractmethod set_base_placement(placement)
Places the robot base at placement.
- Parameters:
placement (timor.utilities.transformation.TransformationLike) – 4x4 Transformation
- Return type:
None
- abstractmethod static_torque(q, f_ext=None)
Computes the static torques in the robot joints composed of g(q) and tau_ext
- Parameters:
q (numpy.ndarray) – Current joint position
f_ext – External forces
- Returns:
n_joints x 1 array of torques
- Return type:
- tau_in_torque_limits(tau)
Returns true if the torque vector tau is within the torque limits of the robot.
We assume that the torque limits of the robot are symmetric (tau_min = -tau_max)
- Parameters:
tau (numpy.ndarray) – A torque vector of dimension dof x 1. The limits will be checked against abs(tau)
- Return type:
- property tcp_acceleration: numpy.ndarray
- Abstractmethod:
- Return type:
Returns the current tcp acceleration as 6x1 numpy array
- property tcp_velocity: numpy.ndarray
- Abstractmethod:
- Return type:
Returns the current tcp velocity as 6x1 numpy array
- abstractmethod update_configuration(q, dq=None, ddq=None)
Updates the current joint angles/configuration.
- Parameters:
q (numpy.ndarray) – Dimension dofx1.
dq (numpy.ndarray) – Velocities, optional
ddq (numpy.ndarray) – Acceleration, optional
- Return type:
None
- velocities: numpy.ndarray
- abstractmethod visualize(viz, *args, **kwargs)
Interface to plot the robot in a meshcat visualizer
- Parameters:
viz (pinocchio.visualize.MeshcatVisualizer)
- Return type:
pinocchio.visualize.MeshcatVisualizer
- timor.Robot.default_ik_cost_function(robot, q, goal)
The default cost function to evaluate the quality of an inverse kinematic solution.
This method returns the weighted sum of the translation and rotation error between the current and the goal, where the 1m of displacement equals a cost of one and 180 degree of orientation error equals a cost of 1. Some short-cuts are taken to reduce object creation via timor’s utilities and acceleration tested in test_default_ik_cost_function.
- Parameters:
robot (RobotBase) – The robot to evaluate the solution for
q (numpy.ndarray) – The current joint configuration
goal (timor.utilities.transformation.Transformation) – The goal transformation
- Return type: