Module pinocchio_utils

Important

The pinocchio_utils module is deprecated!

It has been moved to the robot_properties_fingers package. Please update your code to use the version from that package. The one here will be removed soon.

Wrappers around Pinocchio for easy forward and inverse kinematics.

class trifinger_simulation.pinocchio_utils.Kinematics(finger_urdf_path, tip_link_names)[source]

Forward and inverse kinematics for arbitrary Finger robots.

Provides forward and inverse kinematics functions for a Finger robot with arbitrarily many independent fingers.

Parameters:

  • finger_urdf_path (Union[str, os.PathLike]) – Path to the URDF file describing the robot.

  • tip_link_names (Iterable[str]) – Names of the finger tip frames, one per finger.

forward_kinematics(joint_positions, joint_velocities=None)[source]

Compute end-effector positions and velocities.

Compute position and optionally velocity of the end-effector(s) given angular joint positions/velocities.

Parameters:

  • joint_positions (List[ndarray]) – Flat list of angular joint positions.

  • joint_velocities (ndarray | None) – Optional. Flat list of angular joint velocities.

Returns:

List of end-effector positions. Each position is given as an np.array with x,y,z positions. If joint positions and velocities are given: Tuple with (i) list of end-effector positions and (ii) list of end-effector velocities. Each position and velocity is given as an np.array with x,y,z components.

Return type:

If only joint positions are given

inverse_kinematics(tip_target_positions, joint_angles_guess, tolerance=0.005, max_iterations=1000)[source]

Inverse kinematics for the whole manipulator.

Parameters:
Returns:

First element is the joint configuration, second element is a list of (x,y,z)-errors of the tip positions.

Return type:

Tuple[ndarray, List[ndarray]]

inverse_kinematics_one_finger(finger_idx, tip_target_position, joint_angles_guess, tolerance=0.005, max_iterations=1000)[source]

Inverse kinematics for a single finger.

Parameters:

  • finger_idx (int) – Index of the finger.

  • tip_target_positions – Target position for the finger tip in world frame.

  • joint_angles_guess (ndarray) – Initial guess for the joint angles.

  • tolerance (float) – Position error tolerance. Stop if the error is less then that.

  • max_iterations (int) – Max. number of iterations.

  • tip_target_position (ndarray)

Returns:

First element is the joint configuration (for joints that are not part of the specified finger, the values from the initial guess are kept). Second element is (x,y,z)-error of the tip position.

Return type:

Tuple[ndarray, ndarray]