Introduction

Inverse kinematics (IK) is the problem of computing motions (in Pink: velocities) that achieve a given set of tasks, such as putting a foot on a surface, moving the center of mass to a target location, etc.

This documentation assumes you are already familiar with task-based inverse kinematics. You can check out for instance this post on inverse kinematics for a general introduction.

Notations

In Pink, we adopt the subscript right-to-left convention for transforms, and superscript notation to indicate the frame of a motion or force vector:

Quantity

Notation

Affine transform from frame \(A\) to frame \(B\)

\(T_{BA}\)

Body angular velocity of frame \(A\) in frame \(B\)

\({}^A \omega_{BA}\)

Position of frame \(B\) in frame \(A\)

\({}^A p_B\)

Rotation matrix from frame \(A\) to frame \(B\)

\(R_{BA}\)

Spatial angular velocity of frame \(A\) in frame \(B\)

\({}^B \omega_{BA}\)

World frame (inertial)

\(W\)

With these notations frame transforms can be read left to right, for example:

\begin{align} T_{CA} & = T_{CB} T_{BA} & {}^{B} \omega & = R_{BA} {}^{A} \omega & {}^B p_C & = R_{BA} {}^A p_C + {}^B p_A \end{align}

See also this spatial algebra cheat sheet.

Configuration

Configuration space of a robot model.

Pink uses Pinocchio for forward kinematics. A Configuration is a pair of Pinocchio model and data where forward kinematics have been run, indicating that frame transforms and frame Jacobians used for IK can be queried.

class pink.configuration.Configuration(model, data, q, copy_data=True, forward_kinematics=True, collision_model=None, collision_data=None)

Type indicating that configuration-dependent quantities are available.

In Pink, this type enables access to frame transforms and frame Jacobians. We rely on typing to make sure the proper forward kinematics functions have been called beforehand. In Pinocchio, these functions are:

pin.computeJointJacobians(model, data, configuration)
pin.updateFramePlacements(model, data)

The former computes the full model Jacobian into data.J. (It also computes forward kinematics, so there is no need to further call pin.forwardKinematics(model, data, configuration).) The latter updates frame placements.

Additionally, if a collision model is provided, it is used to evaluate distances between frames by calling the following two functions:

pin.computeCollisions(
    model, data, collision_model, collision_data, q)
pin.updateGeometryPlacements(
    model, data, collision_model, collision_data, q)
data

Data corresponding to Configuration.model.

model

Kinodynamic model.

collision_data

Data corresponding to Configuration.collision_model.

collision_model

Collision model.

q

Configuration vector for the robot model.

check_limits(tol=1e-06, safety_break=True)

Check that the current configuration is within limits.

Parameters:

  • tol (float) – Tolerance in radians.

  • safety_break (bool) – If True, stop execution and raise an exception if the current configuration is outside limits. If False, print a warning and continue execution.

Raises:

NotWithinConfigurationLimits – If the current configuration is outside limits.

Return type:

None

get_frame_jacobian(frame)

Compute the Jacobian matrix of a frame velocity.

Denoting our frame by \(B\) and the world frame by \(W\), the Jacobian matrix \({}_B J_{WB}\) is related to the body velocity \({}_B v_{WB}\) by:

\[{}_B v_{WB} = {}_B J_{WB} \dot{q}\]
Parameters:

frame (str) – Name of the frame, typically a link name from the URDF.

Return type:

ndarray

Returns:

Jacobian \({}_B J_{WB}\) of the frame.

When the robot model includes a floating base (pin.JointModelFreeFlyer), the configuration vector \(q\) consists of:

  • q[0:3]: position in [m] of the floating base in the inertial frame, formatted as \([p_x, p_y, p_z]\).

  • q[3:7]: unit quaternion for the orientation of the floating base in the inertial frame, formatted as \([q_x, q_y, q_z, q_w]\).

  • q[7:]: joint angles in [rad].

get_transform(source, dest)

Get the pose of a frame with respect to another frame.

Parameters:

  • source (str) – Name of the frame to get the pose of.

  • dest (str) – Name of the frame to get the pose in.

Return type:

SE3

Returns:

Current transform from the source frame to the dest frame.

Raises:

KeyError – if any of the frame names is not found in the model.

get_transform_frame_to_world(frame)

Get the pose of a frame in the current configuration.

Parameters:

frame (str) – Name of a frame, typically a link name from the URDF.

Return type:

SE3

Returns:

Current transform from the given frame to the world frame.

Raises:

KeyError – if the frame name is not found in the robot model.

integrate(velocity, dt)

Integrate a velocity starting from the current configuration.

Parameters:

  • velocity – Velocity in tangent space.

  • dt – Integration duration in [s].

Return type:

ndarray

Returns:

New configuration vector after integration.

integrate_inplace(velocity, dt)

Integrate a velocity starting from the current configuration.

Parameters:

  • velocity – Velocity in tangent space.

  • dt – Integration duration in [s].

Return type:

None

update(q=None)

Update configuration to a new vector.

Calling this function runs forward kinematics and computes collision-pair distances, if applicable.

Parameters:

q (Optional[ndarray]) – New configuration vector.

Return type:

None