pinocchio#

class mplib.kinematics.pinocchio.PinocchioModel#

Bases: pybind11_object

Pinocchio model of an articulation

See https://github.com/stack-of-tasks/pinocchio

__init__(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
urdf_filename: str,
*,
gravity: numpy.ndarray[numpy.float64[3, 1]] = array([0., 0., -9.81]),
verbose: bool = False,
) → None#

Construct a Pinocchio model from the given URDF file.

Parameters:

  • urdf_filename – path to the URDF file

  • gravity – gravity vector, by default is [0, 0, -9.81] in -z axis

  • verbose – print debug information. Default: False.

compute_IK_CLIK(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
index: int,
pose: mplib.pymp.Pose,
q_init: numpy.ndarray[numpy.float64[m, 1]],
mask: list[bool] = [],
eps: float = 1e-05,
max_iter: int = 1000,
dt: float = 0.1,
damp: float = 1e-12,
) → tuple[numpy.ndarray[numpy.float64[m, 1]], bool, numpy.ndarray[numpy.float64[6, 1]]]#

Compute the inverse kinematics using close loop inverse kinematics.

Parameters:

  • index – index of the link (in the order you passed to the constructor or the default order)

  • pose – desired pose of the link

  • q_init – initial joint configuration

  • mask – if the value at a given index is True, the joint is not used in the IK

  • eps – tolerance for the IK

  • max_iter – maximum number of iterations

  • dt – time step for the CLIK

  • damp – damping for the CLIK

Returns:

joint configuration

compute_IK_CLIK_JL(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
index: int,
pose: mplib.pymp.Pose,
q_init: numpy.ndarray[numpy.float64[m, 1]],
q_min: numpy.ndarray[numpy.float64[m, 1]],
q_max: numpy.ndarray[numpy.float64[m, 1]],
eps: float = 1e-05,
max_iter: int = 1000,
dt: float = 0.1,
damp: float = 1e-12,
) → tuple[numpy.ndarray[numpy.float64[m, 1]], bool, numpy.ndarray[numpy.float64[6, 1]]]#

The same as compute_IK_CLIK() but with it clamps the joint configuration to the given limits.

Parameters:

  • index – index of the link (in the order you passed to the constructor or the default order)

  • pose – desired pose of the link

  • q_init – initial joint configuration

  • q_min – minimum joint configuration

  • q_max – maximum joint configuration

  • eps – tolerance for the IK

  • max_iter – maximum number of iterations

  • dt – time step for the CLIK

  • damp – damping for the CLIK

Returns:

joint configuration

compute_forward_kinematics(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
qpos: numpy.ndarray[numpy.float64[m, 1]],
) → None#

Compute forward kinematics for the given joint configuration.

If you want the result you need to call get_link_pose()

Parameters:

qpos – joint configuration. Needs to be full configuration, not just the movegroup joints.

compute_full_jacobian(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
qpos: numpy.ndarray[numpy.float64[m, 1]],
) → None#

Compute the full jacobian for the given joint configuration. Note you need to call computeForwardKinematics() first. If you want the result you need to call get_link_jacobian()

Parameters:

qpos – joint configuration. Needs to be full configuration, not just the movegroup joints.

Compute the jacobian of the given link. Note you need to call computeForwardKinematics() first.

Parameters:

  • qpos – joint configuration. Needs to be full configuration, not just the movegroup joints.

  • index – index of the link (in the order you passed to the constructor or the default order)

  • local – if True return the jacobian w.r.t. the instantaneous local frame of the link

Returns:

6 x n jacobian of the link

static create_from_urdf_string(
urdf_string: str,
*,
gravity: numpy.ndarray[numpy.float64[3, 1]] = array([0., 0., -9.81]),
verbose: bool = False,
) → mplib.pymp.kinematics.pinocchio.PinocchioModel#

Constructs a PinocchioModel from URDF string

Parameters:

  • urdf_string – URDF string (without visual/collision elements for links)

  • gravity – gravity vector, by default is [0, 0, -9.81] in -z axis

  • verbose – print debug information. Default: False.

Returns:

a unique_ptr to PinocchioModel

Get the all adjacent link names.

Returns:

adjacent link names as a set of pairs of strings

get_chain_joint_index(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
end_effector: str,
) → list[int]#

Get the joint indices of the joints in the chain from the root to the given link.

Parameters:

index – index of the link (in the order you passed to the constructor or the default order)

Returns:

joint indices of the joints in the chain

get_chain_joint_name(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
end_effector: str,
) → list[str]#

Get the joint names of the joints in the chain from the root to the given link.

Parameters:

index – index of the link (in the order you passed to the constructor or the default order)

Returns:

joint names of the joints in the chain

get_joint_dim(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
index: int,
user: bool = True,
) → int#

Get the dimension of the joint with the given index.

Parameters:

  • index – joint index to query

  • user – if True, the joint index follows the order you passed to the constructor or the default order

Returns:

dimension of the joint with the given index

get_joint_dims(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
user: bool = True,
) → numpy.ndarray[numpy.int32[m, 1]]#

Get the dimension of all the joints. Again, Pinocchio might split a joint into multiple joints.

Parameters:

user – if True, we get the dimension of the joints in the order you passed to the constructor or the default order

Returns:

dimention of the joints

get_joint_id(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
index: int,
user: bool = True,
) → int#

Get the id of the joint with the given index.

Parameters:

  • index – joint index to query

  • user – if True, the joint index follows the order you passed to the constructor or the default order

Returns:

id of the joint with the given index

get_joint_ids(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
user: bool = True,
) → numpy.ndarray[numpy.int32[m, 1]]#

Get the id of all the joints. Again, Pinocchio might split a joint into multiple joints.

Parameters:

user – if True, we get the id of the joints in the order you passed to the constructor or the default order

Returns:

id of the joints

get_joint_limit(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
index: int,
user: bool = True,
) → numpy.ndarray[numpy.float64[m, n]]#

Get the limit of the joint with the given index.

Parameters:

  • index – joint index to query

  • user – if True, the joint index follows the order you passed to the constructor or the default order

Returns:

limit of the joint with the given index

get_joint_limits(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
user: bool = True,
) → list[numpy.ndarray[numpy.float64[m, n]]]#

Get the limit of all the joints. Again, Pinocchio might split a joint into multiple joints.

Parameters:

user – if True, we get the limit of the joints in the order you passed to the constructor or the default order

Returns:

limit of the joints

get_joint_names(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
user: bool = True,
) → list[str]#

Get the name of all the joints. Again, Pinocchio might split a joint into multiple joints.

Parameters:

user – if True, we get the name of the joints in the order you passed to the constructor or the default order

Returns:

name of the joints

get_joint_parent(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
index: int,
user: bool = True,
) → int#

Get the parent of the joint with the given index.

Parameters:

  • index – joint index to query

  • user – if True, the joint index follows the order you passed to the constructor or the default order

Returns:

parent of the joint with the given index

get_joint_parents(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
user: bool = True,
) → numpy.ndarray[numpy.int32[m, 1]]#

Get the parent of all the joints. Again, Pinocchio might split a joint into multiple joints.

Parameters:

user – if True, we get the parent of the joints in the order you passed to the constructor or the default order

Returns:

parent of the joints

get_joint_type(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
index: int,
user: bool = True,
) → str#

Get the type of the joint with the given index.

Parameters:

  • index – joint index to query

  • user – if True, the joint index follows the order you passed to the constructor or the default order

Returns:

type of the joint with the given index

get_joint_types(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
user: bool = True,
) → list[str]#

Get the type of all the joints. Again, Pinocchio might split a joint into multiple joints.

Parameters:

user – if True, we get the type of the joints in the order you passed to the constructor or the default order

Returns:

type of the joints

Get the leaf links (links without child) of the kinematic tree.

Returns:

list of leaf links

Get the jacobian of the given link. You must call compute_full_jacobian() first.

Parameters:

  • index – index of the link (in the order you passed to the constructor or the default order)

  • local – if True, the jacobian is expressed in the local frame of the link, otherwise it is expressed in the world frame

Returns:

6 x n jacobian of the link

Get the name of all the links.

Parameters:

user – if True, we get the name of the links in the order you passed to the constructor or the default order

Returns:

name of the links

Get the pose of the given link in robot’s base (root) frame.

Parameters:

index – index of the link (in the order you passed to the constructor or the default order)

Returns:

pose of the link in robot’s base (root) frame.

get_random_configuration(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
) → numpy.ndarray[numpy.float64[m, 1]]#

Get a random configuration.

Returns:

random joint configuration

print_frames(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
) → None#

Frame is a Pinocchio internal data type which is not supported outside this class.

set_joint_order(
self: mplib.pymp.kinematics.pinocchio.PinocchioModel,
names: list[str],
) → None#

Pinocchio might have a different joint order or it might add additional joints.

If you do not pass the the list of joint names, the default order might not be the one you want.

Parameters:

names – list of joint names in the order you want

Pinocchio might have a different link order or it might add additional links.

If you do not pass the the list of link names, the default order might not be the one you want.

Parameters:

names – list of link names in the order you want