robot_properties_fingers.pinocchio_utils module

Wrappers around Pinocchio for easy forward and inverse kinematics.

class robot_properties_fingers.pinocchio_utils.Kinematics(finger_urdf_path: str | PathLike, tip_link_names: Iterable[str])

Bases: object

Forward and inverse kinematics for arbitrary Finger robots.

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

forward_kinematics(joint_positions: List[ndarray], joint_velocities: ndarray | None = None) → List[ndarray] | Tuple[List[ndarray], List[ndarray]]

Compute end-effector positions and velocities.

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

Parameters:

• joint_positions – Flat list of angular joint positions.

• joint_velocities – 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: Iterable[ndarray], joint_angles_guess: ndarray, tolerance: float = 0.005, max_iterations: int = 1000) → Tuple[ndarray, List[ndarray]]

Inverse kinematics for the whole manipulator.

Parameters:

• tip_target_positions – List of finger tip target positions, one for each finger.

• joint_angles_guess – See inverse_kinematics_one_finger().

• tolerance – See inverse_kinematics_one_finger().

• max_iterations – See inverse_kinematics_one_finger().

Returns:

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

inverse_kinematics_one_finger(finger_idx: int, tip_target_position: ndarray, joint_angles_guess: ndarray, tolerance: float = 0.005, max_iterations: int = 1000) → Tuple[ndarray, ndarray]

Inverse kinematics for a single finger.

Parameters:

• finger_idx – Index of the finger.

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

• joint_angles_guess – Initial guess for the joint angles.

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

• max_iterations – Max. number of iterations.

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.