pyrcf.utils.kinematics_dynamics.pinocchio_interface

Provides utilities for kinematics and dynamics parameters retrievals.

This is a standalone file. pypi dependencies: [numpy, pin]

FEATURES:

Retrieve all dynamics and kinematics info for any robot (urdf).
All methods become available after a single call to the update() method of the class, which
will compute all required kinematics and dynamics data, which can be retreived using the other methods in the class.
Handles continuous joints in addition to revolute and prismatic joints.
Extends the default pinocchio.RobotWrapper class, so all methods from base class still
available (if required).

Attributes

`QuatType`	Numpy array representating quaternion in format [x,y,z,w]
`Vector3D`	Numpy array representating 3D cartesian vector in format [x,y,z]

Classes

PinocchioInterface

A pinocchio.RobotWrapper extension to get robot kinematics and dynamics values easily.

Module Contents

pyrcf.utils.kinematics_dynamics.pinocchio_interface.QuatType: TypeAlias = np.ndarray: Numpy array representating quaternion in format [x,y,z,w]

pyrcf.utils.kinematics_dynamics.pinocchio_interface.Vector3D: TypeAlias = np.ndarray: Numpy array representating 3D cartesian vector in format [x,y,z]

class pyrcf.utils.kinematics_dynamics.pinocchio_interface.PinocchioInterface(urdf_filename: str, floating_base: bool = True, verbose: bool = True, ee_names: List[str] = None)

Bases: pinocchio.RobotWrapper

A pinocchio.RobotWrapper extension to get robot kinematics and dynamics values easily.

This class cannot deal with passive joints, spherical joints or other composite joints. It works fine with regular revolute and prisimatic joints, as well as handles continuous joints.

static configuration_handle_continuous_joints(q: numpy.ndarray, continuous_joint_q_ids: Sequence[float]) → numpy.ndarray

Static method to update the provided pinocchio q vector to handle continuous joints.

This is to be used when the original indices are filled with the joint position values. These will then be replaced with cos(theta) and sin(theta) values in sequence at each of these indices (as required by pinocchio) for continuous joints only.

Parameters:

q (np.ndarray) – The original values where the values at indices continuous_joint_q_ids is filled with the actual value of joint position encoder.
continuous_joint_q_ids (Sequence[float]) – The indices in the q vector which are continuous joints.

Returns:

Updated q vector with appropriate 2D representation for continuous joints.

Return type:

np.ndarray

static recover_generalised_pos_from_pinocchio_configuration(q: numpy.ndarray, continuous_joint_q_ids: Sequence[float] = None) → numpy.ndarray

Static method to get the generalised coordinate position vector from pinocchio q vector.

This function is mainly for handling situations where the robot has continuous joints. Pinocchio uses a 2D representation for a single continuous joint, and this function recovers single joint position values for such joints.

Parameters:

q (np.ndarray) – Configuration vector in the format used by Pinocchio.
continuous_joint_q_ids (Sequence[float], optional) – The joint ids of continuous joints. Defaults to [].

Returns:

Output generalised coordinate vector after removing pinocchio’s: continuous joint representation.

Return type:

np.ndarray

floating_base

name: Name of the robot model.

total_mass

num_joints

joint_names

joint_lower_limits: List[float]

joint_upper_limits: List[float]

joint_neutral_position

joint_velocity_limit: List[float]

actuated_joint_names: List[str] = []

actuated_joint_q_ids: List[int] = []: index of joints in the generalised coordinate vector (q)

q: The pinocchio q vector (generalised coordinates).

v: The pinocchio v vector (generalised velocity).

a

never filled!

Type:: The pinocchio a output vector (generalised acceleration). NOTE

actuated_joint_v_ids: List[int] = []: index of joints in the generalised velocity vector (v)

continuous_joint_q_ids: List[int] = []: Indices corresponding to continuous joints in the pinocchio configuration vector q. In pinocchio, continuous joints are represented using 2 values [cos(theta), sin(theta)]. This list contains the index to the first (cos) value (add 1 to the index to get the next).

continuous_joint_names: List[str] = []

num_of_actuated_joints

additional_dims_

expected_nq_

actuated_joint_name_to_q_index: Mapping[str, int]: Mapping from joint name to index of the joint configuration value in the pinocchio q vector.

actuated_joint_name_to_v_index: Mapping[str, int]: Mapping from joint name to index of the joint velocity value in the pinocchio v vector.

actuated_joint_lower_limits

actuated_joint_upper_limits

actuated_joint_neutral_position

actuated_joint_limits: mapping from {‘joint_name’ -> [lower_lim, upper_lim]}

num_of_frames

frame_names

frame_ids

ee_names: Names of frames that are used as end-effectors.

print_model_info()

update(global_base_position: Vector3D, global_base_quaternion: QuatType, local_base_velocity_linear: Vector3D, local_base_velocity_angular: Vector3D, joint_positions: numpy.ndarray, joint_velocities: numpy.ndarray, joint_order: List[str] = None)

Do all forward kinematics and dynamics computations for the given robot states.

Also updates the internal configuration vectors (self.q, self.v).

Parameters:

global_base_position (Vector3D) – Position of robot base in the world.
global_base_quaternion (QuatType) – Orientation quaternion (x,y,z,w) of the robot base in the world.
local_base_velocity_linear (Vector3D) – Linear velocity of the robot base in the local base frame.
local_base_velocity_angular (Vector3D) – Angular velocity of the robot base in the local base frame.
joint_positions (np.ndarray) – Current joint positions of the robot.
joint_velocities (np.ndarray) – Current joint velocities of the robot.
joint_order (List[str], optional) – Joint names in the order the position values were given. If None provided, uses default order found by pinocchio (urdf order).

get_frame_id(frame_name: str) → int

get_com_position() → Vector3D

get_com_velocity() → Vector3D

get_centroidal_mass() → float

get_gravity_vector() → numpy.ndarray

get_centroidal_inertia() → numpy.ndarray

get_centroidal_momentum_map() → numpy.ndarray

get_centroidal_momentum_map_linear() → numpy.ndarray

get_centroidal_momentum_map_angular() → numpy.ndarray

get_inertia_matrix() → numpy.ndarray

get_nonlinear_effects() → numpy.ndarray

get_joint_positions(joint_names: List[str] = None) → numpy.ndarray

Get the current joint positions using the latest joint configuration vector.

This method also handles continuous joints and extracts joint position values for continuous joints from the 2D configuration representation used in pinocchio.

Parameters:: joint_names (List[str], optional) – Optional list of joints/joint order. Defaults to None (uses default joint order).
Returns:: Values of joint position/orientation for specified/all joints.
Return type:: np.ndarray

get_joint_positions_from_configuration(q: numpy.ndarray, joint_names: List[str] = None) → numpy.ndarray

Get the joint positions from the provided pinocchio configuration vector.

This method also handles continuous joints and extracts joint position values for continuous joints from the 2D configuration representation used in pinocchio.

NOTE: This assumes the provided q vector has the same order as the self.q values in this class.

Parameters:

q (np.ndarray) – Pinocchio configuration vector from which joint values are to be extracted. Assumes the provided q vector has the same order as the self.q values in this class.
joint_names (List[str], optional) – Optional list of joints/joint order. Defaults to None (uses default joint order).

Returns:

Values of joint position/orientation for specified/all joints.

Return type:

np.ndarray

get_generalised_coordinates_from_q(q: numpy.ndarray) → numpy.ndarray

Get the generalised coordinate representation for the state of the robot.

This is going to be ordered joint position values in case of fixed-base robots, and concatenation of base position, base quaternion (x,y,z,w) and joint positions for floating base systems.

Parameters:: q (np.ndarray) – Input Pinocchio q vector.
Returns:: output generalised coordinate state representation of this robot.
Return type:: np.ndarray

get_pinocchio_q_and_v_from_robot_state(global_base_position: Vector3D, global_base_quaternion: QuatType, local_base_velocity_linear: Vector3D, local_base_velocity_angular: Vector3D, joint_positions: numpy.ndarray, joint_velocities: numpy.ndarray, joint_order: List[str] = None) → Tuple[numpy.ndarray, numpy.ndarray]

Get pinocchio configuration vector (q) and velocity (v) from robot state data.

Also handles continuous joints.

Parameters:

global_base_position (Vector3D) – Position of robot base in the world.
global_base_quaternion (QuatType) – Orientation quaternion (x,y,z,w) of the robot base in the world.
local_base_velocity_linear (Vector3D) – Linear velocity of the robot base in the local base frame.
local_base_velocity_angular (Vector3D) – Angular velocity of the robot base in the local base frame.
joint_positions (np.ndarray) – Current joint positions of the robot.
joint_velocities (np.ndarray) – Current joint velocities of the robot.
joint_order (List[str], optional) – Joint names in the order the position values were given. If None provided, uses default order found by pinocchio (urdf order).

Returns:

The q and v vectors that can be used for pinocchio: operations.

Return type:

Tuple[np.ndarray, np.ndarray]

get_frame_position(frame_name: str, reference_frame: str = 'world') → Vector3D

get_global_frame_pose(frame_name: str) → Tuple[Vector3D, QuatType]

get_ee_poses(ee_names: List[str] = None) → List[Tuple[Vector3D, QuatType]]

get_ee_velocities(ee_names: List[str] = None) → List[Tuple[Vector3D, Vector3D]]

get_frame_rotation(frame_name: str, reference_frame: str = 'world') → numpy.ndarray

get_frame_quaternion(frame_name: str, reference_frame: str = 'world') → QuatType

get_frame_transformation(frame_name: str, reference_frame: str = 'world') → numpy.ndarray

get_frame_velocity_linear(frame_name: str, reference_frame: str = 'world') → Vector3D

get_frame_velocity_angular(frame_name: str, reference_frame: str = 'world') → Vector3D

get_frame_acceleration_linear(frame_name: str, reference_frame: str = 'world') → Vector3D

get_frame_acceleration_angular(frame_name: str, reference_frame: str = 'world') → Vector3D

get_frame_jacobian(frame_name: str, reference_frame: str = 'world') → numpy.ndarray

get_frame_jacobian_linear(frame_name: str, reference_frame: str = 'world') → numpy.ndarray

get_frame_jacobian_angular(frame_name: str, reference_frame: str = 'world') → numpy.ndarray