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 note 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}\)

Plücker transform from frame \(A\) to frame \(B\)

\(X_{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} X_{CA} & = X_{CB} X_{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 type.

Pink uses Pinocchio for forward kinematics. It adds to it a Configuration type to indicate that forward kinematics functions have been run, indicating that frame transforms and frame Jacobians used for IK can be queried from the robot’s data.

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

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.

Notes

This class is meant to be used as a subclass of pin.RobotWrapper, not wrap it. However, right now pin.RobotWrapper does not have a shallow copy constructor. TODO(scaron): bring it up upstream.

data

Data corresponding to Configuration.model.

Type:

pinocchio.pinocchio_pywrap.Data

model

Kinodynamic model.

Type:

pinocchio.pinocchio_pywrap.Model

q

Configuration vector for the robot model.

Type:

numpy.ndarray

check_limits(tol=1e-06)

Check that the current configuration is within limits.

Parameters:

tol (float) – Tolerance in radians.

Raises:

NotWithinConfigurationLimits – if the current configuration is within limits.

Return type:

None

get_frame_jacobian(body)

Compute the Jacobian matrix of the body velocity.

This matrix \({}_B J_{WB}\) is related to the body velocity \({}_B v_{WB}\) by:

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

body (str) – Body frame name, typically the link name from the URDF.

Return type:

ndarray

Returns:

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

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_frame_to_world(body)

Get the pose of a body frame in the current configuration.

Parameters:

body (str) – Body frame name, typically the link name from the URDF.

Return type:

SE3

Returns:

Current transform from body frame to world frame.

Raises:

KeyError – if the body 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()

Run forward kinematics from the configuration.

Return type:

None