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 of a robot model.

Pink uses Pinocchio for forward kinematics. A Configuration holds a Pinocchio model and data for this model where forward kinematics have been run. This means that the geometric state of the model has been computed, and quantities such as frame transforms and frame Jacobians are available.

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)

collision_data: Data corresponding to Configuration.collision_model.

collision_model: Collision model.

data: Data corresponding to Configuration.model.

model: Kinodynamic 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

Task formalism

The task function approach followed in Pink, and many works before it, was originally formalized by Samson, Espiau and Le Borgne in [Samson1991].

In Pink, all kinematic tasks derive from the Task base class.

The formalism used in the library is written down in this note on task-based inverse kinematics. As of February 2022, it hasn’t been updated with the proper dimensional analysis, but the core concepts and notations are there.

class pink.tasks.task.Task(cost=None, gain=1.0, lm_damping=0.0)

Abstract base class for kinematic tasks.

cost: cost vector with the same dimension as the error of the task. Its units depends on the error as well.

gain: Task gain \(\alpha \in [0, 1]\) for additional low-pass filtering. Defaults to 1.0 (no filtering) for dead-beat control.

lm_damping: Unitless scale of the Levenberg-Marquardt (only when the error is large) regularization term, which helps when targets are unfeasible. Increase this value if the task is too jerky under unfeasible targets, but beware that a larger damping slows down the task.

abstractmethod compute_error(configuration)

Compute the task error function.

The error function \(e(q) \in \mathbb{R}^{k}\) is the quantity that the task aims to drive to zero (\(k\) is the dimension of the task). It appears in the first-order task dynamics:

\[J(q) \Delta q = -\alpha e(q)\]

The Jacobian matrix \(J(q) \in \mathbb{R}^{k \times n_v}\), with \(n_v\) the dimension of the robot’s tangent space, is the derivative of the task error \(e(q)\) with respect to the configuration \(q \in \mathbb{R}^{n_q}\). This Jacobian is implemented in Task.compute_jacobian(). Finally, the configuration displacement \(\Delta q\) is the output of inverse kinematics.

In the first-order task dynamics, the error \(e(q)\) is multiplied by the task gain \(\alpha \in [0, 1]\). This gain can be one for dead-beat control (i.e. converge as fast as possible, but might be unstable as it neglects our first-order approximation), but it can also be lower a slower task (similar to low-pass filtering).

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Return type:: ndarray
Returns:: Task error vector \(e(q)\).

abstractmethod compute_jacobian(configuration)

Compute the task Jacobian at a given configuration.

The task Jacobian \(J(q) \in \mathbb{R}^{k \times n_v}\) is the first-order derivative of the error \(e(q) \in \mathbb{R}^{k}\) that defines the task, with \(k\) the dimension of the task and \(n_v\) the dimension of the robot’s tangent space.

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Return type:: ndarray
Returns:: Task Jacobian \(J(q)\).

compute_qp_objective(configuration)

Compute the matrix-vector pair \((H, c)\) of the QP objective.

This pair is such that the contribution of the task to the QP objective of the IK is:

\[\| J \Delta q + \alpha e \|_{W}^2 = \frac{1}{2} \Delta q^T H \Delta q + c^T q\]

The weight matrix \(W \in \mathbb{R}^{k \times k}\) weighs and normalizes task coordinates to the same unit. The unit of the overall contribution is [cost]^2. The configuration displacement \(\Delta q\) is the output of inverse kinematics (we divide it by \(\Delta t\) to get a commanded velocity).

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Return type:: Tuple[ndarray, ndarray]
Returns:: Pair \((H(q), c(q))\) of Hessian matrix and linear vector of the QP objective.