timor.Robot =========== .. py:module:: timor.Robot Classes ------- .. autoapisummary:: timor.Robot.PinRobot timor.Robot.RobotBase Functions --------- .. autoapisummary:: timor.Robot.default_ik_cost_function Module Contents --------------- .. py:class:: PinRobot(*, wrapper, home_configuration = None, base_placement = None, rng = None) .. autoapi-inheritance-diagram:: timor.Robot.PinRobot :parts: 1 :private-bases: 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 :param wrapper: Pinocchio robot wrapper. .. py:method:: __check_collisions(task, return_at_first, safety_margin = 0) Checks all collision pairs or all collisions pairs until the first colliding one is found. :param task: The task to check for collisions :param return_at_first: If true, this function behaves like a "has_collision". If false, it behaves like a "get_all_collisions" function. :param safety_margin: 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). :return: Bool if return_at_first is true, otherwise a list of all colliding pairs. .. py:method:: __deepcopy__(memodict={}) :abstractmethod: Pin Robot cannot be deep-copied without losing the information on how bodies are connected. .. py:method:: __getstate__() Custom getstate (used for pickling) for pinocchio robot in order to enable multiprocessing. Geometric data from pinocchio is no serializeable .. py:method:: __setstate__(state) Unpickle. .. py:attribute:: __slots__ :value: () .. py:method:: _add_body(body, parent_joint, parent_frame_id) Adds a single body to the robot. Note that collision and visual data are not updated! :param body: A body wrapper :param parent_joint: The index or name of the parent joint :param parent_frame_id: The id of the parent frame of the body (important to calculate body placement) :return: The index of the new body frame and indices for the newly added collision geometries .. py:method:: _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. :param joint: A pinocchio joint model instance :param parent: Either the index or the name of the parent joint to which the new one shall be appended :param bodies: All bodies attached to the joint. To add N bodies, there need to be N-1 fixed joints. :param fixed_joints: Fixed joints attached to the joint. Assumes bodies and fixed joints are added in the order b1-fj1-b2-fx2-...-bn :param inertia: The inertia attached to the joint. If not provided, will be auto-calculated by the bodies added. :param update_kinematics: 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. :param update_home_collisions: 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. :param 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 :return: Index of the new joint .. py:method:: _init_collision_pairs() Adds all collision pairs to the robot. .. py:attribute:: _known_ik_solvers .. py:attribute:: _name :type: str .. py:method:: _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. .. py:method:: _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. .. py:attribute:: _rng :type: numpy.random.Generator .. py:attribute:: _tcp .. py:method:: _update_collision_placement() Updates the placement of collision geometry objects .. py:method:: _update_geometry_placement() Updates the collision (for collision detection) and visual (for plotting) geometry placements. .. py:method:: _update_kinematic() Can be called after changing the robot structure. This method updates the kinematic model of the robot .. py:method:: _update_visual_placement() Updates the placement of visual geometry objects .. py:attribute:: accelerations :type: numpy.ndarray .. py:method:: add_tcp_frame(parent_joint, placement) Adds a Tool Center Point that will be used for inverse kinematic calculations. :param parent_joint: The joint the TCP is attached to :param placement: Relative placement of the TCP in the parent joint frame :return: The index of the TCP frame .. py:attribute:: collision :type: pinocchio.GeometryModel .. py:attribute:: collision_data :type: pinocchio.GeometryData .. py:method:: collision_info(task) Calculates all collisions in the task and prints human-readable information. :param task: Task to test for collisions .. py:property:: collision_objects :type: 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. .. py:property:: collision_pairs :type: numpy.ndarray Uses the collision_objects property and returns all pairs that are in collision :return: An nx2 numpy array of collision pair inidices, where n is the number of pairs in collision .. py:method:: collisions(task, safety_margin = 0) Returns all collisions that appear between the robot and itself or obstacles in the task. :param task: The task to check for collisions :param safety_margin: The safety margin is a padding added to all obstacles to increase needed separation. :return: A list of tuples (robot, obstacle) for robot an obstacle in collision .. py:attribute:: configuration :type: numpy.ndarray .. py:attribute:: data :type: pinocchio.Data .. py:property:: ddq :type: numpy.ndarray Alias for self.accelerations .. py:property:: dof :type: int Number of actuated degrees of freedom. .. py:property:: dq :type: numpy.ndarray Alias for self.velocities .. py:method:: 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. :param tau: Torques commanded :param q: Joint position; defaults to current position (self.configuration) :param dq: Joint velocity; defaults to current velocity (self.velocities) :param motor_inertia: Consider motor inertias :param friction: Consider joint friction :return: ddq the resulting joint accelerations .. py:method:: fk(configuration = None, kind = 'tcp', collision = False, visual = False) Calculate the forward kinematics (joint state -> end-effector position). :param configuration: Joint configuration the fk should be given for. If None, will take current configuration. :param kind: Either 'tcp', 'joints' or 'full'. :param collision: Perform updates on the collision geometry objects placements :param visual: Perform updates on the visual geometry objects placements :return: np. array of either dim 4x4 (kind tcp) or numJointsx4x4 (kind joints) or numFramesx4x4 (kind full) .. py:method:: friction(dq) Computes the forces due to friction .. py:method:: from_urdf(urdf_file, package_dir, **kwargs) :classmethod: Utility wrapper to load information about a robot from URDF and build the according PinRobot. .. py:method:: has_collisions(task, safety_margin = 0) Returns true if there is at least one collision in the task or one self-collision. :param task: The task the robot is in. :param safety_margin: The safety margin is a padding added to all obstacles to increase needed separation. :return: Boolean indicator whether there are any collisions. .. py:method:: 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. .. py:method:: 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. :param motor_inertia: 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. :param friction: Whether to consider friction compensating torques :param eef_wrench: [f_x, f_y, f_z, tau_x, tau_y, tau_z] acting on the end-effector :param eef_wrench_frame: Frame the eef_wrench is described in (still acting on eef position!); for now only "world" accepted. TODO : Extend with future frame definitions .. py:method:: ik(eef_pose, *, ik_method = None, **kwargs) Interface for inverse kinematics solver. :param eef_pose: The desired 4x4 placement of the end effector :param ik_method: The method to use for the inner loop. Defaults to jacobian for robots with >=6 dof and scipy for robots with <6 dof. :return: A tuple of (q_solution, success [boolean]) .. py:method:: 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 :param eef_pose: Desired end effector pose with tolerances. :param info: An IKMeta object that contains relevant meta information about the IK problem and can be adapted during the inner IK process. :param q_init: The joint configuration to start with. :param callbacks: An iterable of callbacks that are called after each iteration during optimization. :param damp: Damping for the damped least squares pseudoinverse method :param ik_cost_function: 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. :param convergence_threshold: Threshold for termination. Optimization terminates when the exponentially decaying average of the cost function improvements is below this threshold. :param alpha_average: 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) :param error_term_mask: 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. :param gain: Gain Matrix K determining the step size per joint :math:`K J^{-q} e`. If a scalar is provided, it is interpreted as the same gain for all joints. :param 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. :param tcp_or_world_frame: 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. :return: 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. .. py:method:: 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. :param eef_pose: Desired end effector pose with tolerances. :param info: 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). :param q_init: The joint configuration to start with. :param callbacks: An iterable of callbacks that are called after each iteration during optimization. :param damp: Damping for the damped least squares pseudoinverse method. :param ik_cost_function: 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. :param convergence_threshold: Threshold for termination. Optimization terminates when the exponentially decaying average of the cost function improvements is below this threshold. :param alpha_average: This factor controls the exponential decay of the running average computation of cost. :param step_size: Step size for the optimization. :param eps: 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. :param check_self_collisions: If True, self collisions are checked during the optimization and solutions that self-collide are invalid. :param allow_inner_restart: 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. :param weight_translation: Weight for the translation part (L2 distance in meters) of the cost function. :param weight_rotation: Weight for the rotation part (angular distance in rad) of the cost function. :return: A tuple of q_solution [np.ndarray], success [boolean]. If success is False, q_solution is closest found guess according to the cost function. .. py:method:: 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. :param eef_pose: Desired end effector pose with tolerances. :param info: An IKMeta object that contains relevant meta information about the IK problem and can be adapted during the inner IK process. :param q_init: Initial joint angles to start searching from. If None, uses a random configuration :param callbacks: An iterable of callbacks that are called after each iteration during optimization. :param ik_cost_function: 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). :param convergence_threshold: Threshold for termination. Optimization terminates when two consecutive iterates are improving the cost by less than this. :param scipy_kwargs: 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. :return: 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. .. py:property:: joint_acceleration_limits :type: numpy.ndarray Numpy array where arr[0,:] are the lower joint limits and arr[1,:] are the upper acceleration limits. .. py:property:: joint_limits :type: numpy.ndarray Numpy array where arr[0,:] are the lower joint limits and arr[1,:] are the upper joint limits. .. py:property:: joint_torque_limits :type: numpy.ndarray Numpy array where arr[0,:] are the lower joint limits and arr[1,:] are the upper torque limits. .. py:property:: joint_velocity_limits :type: numpy.ndarray Numpy array with upper bound absolute joint velocity limits. .. py:property:: joints :type: List[str] Returns the names of the "real" joints in the robot. (ignoring universe, which is, in pinocchio, a joint) .. py:method:: manipulability_index(q = None) Calculate the Yoshikawa manipulability index at joint position q or current one. :param q: Joint position to calculate at (default current robot configuration) .. py:property:: mass :type: 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 .. py:attribute:: model :type: pinocchio.Model .. py:method:: motor_inertias(ddq) Computes the motor inertias in the joins at a given acceleration .. py:method:: move(displacement) Moves frames, joints and geometries connected to the world. :param displacement: Transformation / transformation transform to move, given in the world/universe frame .. py:property:: name :type: str Unique robot name .. py:property:: parents :type: Dict[str, str] Returns a mapping from a joint to its parent. Assumes there is a unique parent for each joint! .. py:property:: placement :type: timor.utilities.transformation.Transformation The robot's base placement. Wrapped as property so it can only be set using the according methods. .. py:property:: q :type: numpy.ndarray Alias for self.configuration .. py:method:: q_in_joint_limits(q) Returns true if a configuration q is within the joint limits of the robot .. py:method:: random_configuration(rng = None) Generates a random configuration. :param rng: Random number generator to use. If not given, this method uses the pinocchio default. .. py:method:: set_base_placement(placement) Moves the base to desired position .. py:method:: static_torque(q, f_ext = None) Static torques of the robot :param q: Current joint configuration :param f_ext: External forces as array of pin forces :return: n_joints x 1 array of torques .. py:method:: 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) :param tau: A torque vector of dimension dof x 1. The limits will be checked against abs(tau) .. py:property:: tcp The index of the frame indicating the robot's TCP (Tool Center Point). .. py:property:: tcp_acceleration :type: numpy.ndarray Returns the current tcp velocity as 6x1 numpy array - relative to the world coordinate system. :return: Array (ddx, ddy, ddz, dd_alpha, dd_beta, dd_gamma) / dt .. py:property:: tcp_parent :type: str Returns the NAME of the joint the tcp frame is attached to .. py:property:: tcp_velocity :type: numpy.ndarray Returns the current tcp velocity as 6x1 numpy array - per default relative to the world coordinate system. :param 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 :return: Array math:`(\Delta x, \Delta y, \Delta z, \Delta \alpha, \Delta \beta, \Delta \gamma) / \Delta t` .. py:method:: update_configuration(q, dq = None, ddq = None, *, frames = False, geometry = False) Update the robot configuration by updating the joint parameters. :param q: The new configuration in joint space iven as numpy array of joint values. :param dq: Joint velocities, optional :param ddq: Joint acceleration, optional :param frames: If true, frame placements are also updated :param geometry: If true, geometry (visual and collision) placements are updated :return: None .. py:attribute:: velocities :type: numpy.ndarray .. py:attribute:: visual :type: pinocchio.GeometryModel .. py:attribute:: visual_data :type: pinocchio.GeometryData .. py:method:: visualize(visualizer = None, coordinate_systems = None, update_collision_visuals = True) Displays the robot in its current environment using a Meshcat Visualizer in the browser :param visualizer: A meshcat visualizer instance. If none, a new will be created :param coordinate_systems: Can be None, 'tcp', 'joints' or 'full' - the specified coordinates will be drawn (full means all frames, as in fk). :param update_collision_visuals: If true, the collision geometry will be updated as well (but not shown by default). :return: The meshcat visualizer instance used for plotting the robot .. py:method:: 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. :param visualizer: A pinnocchio meshcat visualizer (only tested for this one) .. py:class:: RobotBase(*, name, home_configuration = None, base_placement = None, rng = None) .. autoapi-inheritance-diagram:: timor.Robot.RobotBase :parts: 1 :private-bases: Abstract base class for all robot classes RobotBase Constructor :param home_configuration: Home configuration of the robot (default pin.neutral) :param base_placement: Home configuration of the robot (default pin.neutral) :param rng: 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. .. py:attribute:: __slots__ :value: () .. py:attribute:: _known_ik_solvers .. py:attribute:: _name :type: str .. py:attribute:: _rng :type: numpy.random.Generator .. py:attribute:: accelerations :type: numpy.ndarray .. py:method:: collision_info(task) Calculates all collisions in the task and prints human-readable information. :param task: Task to test for collisions .. py:method:: collisions(task, safety_margin = 0) :abstractmethod: Returns all collisions that appear between the robot and itself or obstacles in the task. :param task: The task to check for collisions :param safety_margin: The safety margin is a padding added to all obstacles to increase needed separation. :return: A list of tuples (robot, obstacle) for robot an obstacle in collision .. py:attribute:: configuration :type: numpy.ndarray .. py:property:: ddq :type: numpy.ndarray Alias for self.accelerations .. py:property:: dof :type: int :abstractmethod: Returns the number of joints in the robot. .. py:property:: dq :type: numpy.ndarray Alias for self.velocities .. py:method:: fk(configuration = None, kind = 'tcp') :abstractmethod: Returns the forward kinematics for the robot as homogeneous transformation. :param configuration: Joint configuration the fk should be given for. If None, will take current configuration. :param kind: Depends on the class, but should at least support 'tcp' and 'joints'. .. py:method:: from_urdf(urdf_file, package_dir, **kwargs) :classmethod: :abstractmethod: Class constructor from urdf file. kwargs should at least handle RobotBase keyword arguments. .. py:method:: has_collisions(task, safety_margin = 0) :abstractmethod: 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. :param task: The task the robot is in. :param safety_margin: The safety margin is a padding added to all obstacles to increase needed separation. :return: Boolean indicator whether there are any collisions. .. py:method:: has_self_collision(q = None) :abstractmethod: Returns true if there are any self collisions in configuration q (defaults to current configuration) .. py:method:: id(q = None, dq = None, ddq = None, **kwargs) :abstractmethod: Inverse Dynamics solver, expected to return torques as 1xnJoints array. Should work with current configuration if not explicitly given q, dq and ddq. .. py:method:: 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. :param eef_pose: The desired 4x4 pose of the end effector. :param allow_random_restart: 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). :param current_best: The current best solution, if any. In case of failure, the best intermediate solution will be returned :param ignore_self_collisions: If True, self collisions will be ignored during the IK. By default, a solution with self collisions will never be considered successful. :param inner_callbacks: Any number of callbacks that will be called after each iteration of the inner loop. :param ik_cost_function: 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. :param ik_method: The method to use for the inner loop. Defaults to 'scipy'. :param joint_mask: 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. :param max_iter: The maximum number of iterations before the algorithm gives up. :param outer_callbacks: Any number of callbacks that will be called after each iteration of the outer loop. :param outer_constraints: 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. :param task: If a task is provided, any of the constraints defined in the task will be considered outer constraints. :param convergence_threshold: A threshold for convergence: When the cost function stops improving by more than this threshold, the IK will terminate even without finding a solution. :param q_init: The joint configuration to start with. If not given, it is up to the downstream method to determine a suitable initial configuration. :param kwargs: Any keyword arguments that are specific to the inner loop method. :return: 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. .. py:method:: 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. :param eef_pose: Desired end effector pose with tolerances. :param info: An IKMeta object that contains relevant meta information about the IK problem and can be adapted during the inner IK process. :param q_init: Initial joint angles to start searching from. If None, uses a random configuration :param callbacks: An iterable of callbacks that are called after each iteration during optimization. :param ik_cost_function: 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). :param convergence_threshold: Threshold for termination. Optimization terminates when two consecutive iterates are improving the cost by less than this. :param scipy_kwargs: 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. :return: 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. .. py:property:: joint_acceleration_limits :type: numpy.ndarray :abstractmethod: Returns an array of shape 1xdof containing the absolute joint acceleration limits .. py:property:: joint_limits :type: numpy.ndarray :abstractmethod: Returns an array of shape 2xdof containing the lower and upper joint position limits .. py:property:: joint_torque_limits :type: numpy.ndarray :abstractmethod: Returns an array of shape 1xdof containing the absolute joint torque limits .. py:property:: joint_velocity_limits :type: numpy.ndarray :abstractmethod: Returns an array of shape 1xdof containing the absolute joint velocity limits .. py:property:: joints :type: List[str] :abstractmethod: Returns a list containing all joint IDs of the robot .. py:method:: manipulability_index(q) :abstractmethod: Calculate the Yoshikawa manipulability index at joint position q or current one. :param q: Joint position to calculate at (default current robot configuration) .. py:property:: mass :type: float :abstractmethod: Total robot mass in kilogram .. py:method:: move(displacement) :abstractmethod: Moves the whole robot by displacement :param displacement: A 4x4 Transformation .. py:property:: name :type: str Unique robot name .. py:property:: parents :type: Dict[str, str] :abstractmethod: Returns a mapping between joints and their parents. The base/world joint is its own parent .. py:property:: placement :type: timor.utilities.transformation.Transformation :abstractmethod: The (base) placement in the world coordinate system (4x4 homogeneous transformation / placement) .. py:property:: q :type: numpy.ndarray Alias for self.configuration .. py:method:: q_in_joint_limits(q) Returns true if a configuration q is within the joint limits of the robot .. py:method:: random_configuration(rng = None) Generates a random configuration. :param rng: Random number generator to use. If not given, this method uses the robot default rng. .. py:method:: set_base_placement(placement) :abstractmethod: Places the robot base at placement. :param placement: 4x4 Transformation .. py:method:: static_torque(q, f_ext=None) :abstractmethod: Computes the static torques in the robot joints composed of g(q) and tau_ext :param q: Current joint position :param f_ext: External forces :return: n_joints x 1 array of torques .. py:method:: 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) :param tau: A torque vector of dimension dof x 1. The limits will be checked against abs(tau) .. py:property:: tcp_acceleration :type: numpy.ndarray :abstractmethod: Returns the current tcp acceleration as 6x1 numpy array .. py:property:: tcp_velocity :type: numpy.ndarray :abstractmethod: Returns the current tcp velocity as 6x1 numpy array .. py:method:: update_configuration(q, dq = None, ddq = None) :abstractmethod: Updates the current joint angles/configuration. :param q: Dimension dofx1. :param dq: Velocities, optional :param ddq: Acceleration, optional .. py:attribute:: velocities :type: numpy.ndarray .. py:method:: visualize(viz, *args, **kwargs) :abstractmethod: Interface to plot the robot in a meshcat visualizer .. py:function:: 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. :param robot: The robot to evaluate the solution for :param q: The current joint configuration :param goal: The goal transformation