Tasks

Definition

All kinematic tasks derive from the Task base class.

The formalism used in this implementation 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.

abstract 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)\).

abstract 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.

See also

Levenberg-Marquardt damping is described in [Sugihara2011]. The dimensional analysis in this class is our own.

Frame task

Frame task implementation.

class pink.tasks.frame_task.FrameTask(frame, position_cost, orientation_cost, lm_damping=0.0, gain=1.0)

Regulate the pose of a robot frame in the world frame.

frame

Frame name, typically the name of a link or joint from the robot description.

transform_target_to_world

Target pose of the frame.

Costs are designed so that errors with varying SI units, here position and orientation displacements, can be cast to homogeneous values. For example, if task “foo” has a position cost of 1.0 and task “bar” a position cost of 0.1, then a 1 [cm] error in task “foo” costs as much as a 10 [cm] error in task “bar”.

Note

Dimensionally, the 6D cost vector is a (normalized) force screw and our objective function is a (normalized) energy.

compute_error(configuration)

Compute frame task error.

This error is a twist \(e(q) \in se(3)\) expressed in the local frame (i.e. it is a body twist). We map it to \(\mathbb{R}^6\) using Pinocchio’s convention where linear coordinates are followed by angular coordinates.

The error is the right-minus difference between the target pose \(T_{0t}\) and current frame pose \(T_{0b}\):

\[e(q) := {}_b \xi_{0b} = -(T_{t0} \boxminus T_{b0}) = -\log(T_{t0} \cdot T_{0b}) = -\log(T_{tb}) = \log(T_{bt})\]

where \(b\) denotes our frame, \(t\) the target frame and \(0\) the inertial frame.

See pink.tasks.task.Task.compute_error() for more context, and [MLT] for details on the right-minus operator.

Parameters:

configuration (Configuration) – Robot configuration \(q\).

Return type:

ndarray

Returns:

Frame task error \(e(q)\).

compute_jacobian(configuration)

Compute the frame task Jacobian.

The task Jacobian \(J(q) \in \mathbb{R}^{6 \times n_v}\) is the derivative of the task error \(e(q) \in \mathbb{R}^6\) with respect to the configuration \(q\). The formula for the frame task is:

\[J(q) = -\text{Jlog}_6(T_{tb}) {}_b J_{0b}(q)\]

The derivation of the formula for this Jacobian is detailed in [FrameTaskJacobian]. See also pink.tasks.task.Task.compute_jacobian() for more context on task Jacobians.

Parameters:

configuration (Configuration) – Robot configuration \(q\).

Return type:

ndarray

Returns:

Jacobian matrix \(J\), expressed locally in the frame.

set_orientation_cost(orientation_cost)

Set a new cost for all 3D orientation coordinates.

Parameters:

orientation_cost (Union[float, Sequence[float], ndarray]) – Contribution of orientation errors to the normalized cost, in \([\mathrm{cost}] / [\mathrm{rad}]\). If this is a vector, the cost is anisotropic and each coordinate corresponds to an axis of the frame.

Return type:

None

set_position_cost(position_cost)

Set a new cost for all 3D position coordinates.

Parameters:

position_cost (Union[float, Sequence[float], ndarray]) – Contribution of position errors to the normalized cost, in \([\mathrm{cost}] / [\mathrm{m}]\). If this is a vector, the cost is anisotropic and each coordinate corresponds to an axis of the frame.

Return type:

None

set_target(transform_target_to_world)

Set task target pose in the world frame.

Parameters:

transform_target_to_world (SE3) – Transform from the task target frame to the world frame.

Return type:

None

set_target_from_configuration(configuration)

Set task target pose from a robot configuration.

Parameters:

configuration (Configuration) – Robot configuration.

Return type:

None

Relative frame task

Relative frame task implementation.

class pink.tasks.relative_frame_task.RelativeFrameTask(frame, root, position_cost, orientation_cost, lm_damping=0.0, gain=1.0)

Regulate the pose of a robot frame relative to another robot frame.

frame

Frame name, typically the name of a link or joint from the robot description.

root

Name of the robot frame the task is relative to.

transform_target_to_root

Target pose of the frame.

compute_error(configuration)

Compute the error twist.

The error is a twist \(e(q) \in se(3)\) expressed in the local frame (i.e., it is a body twist). We map it to \(\mathbb{R}^6\) following Pinocchio’s convention where linear coordinates are followed by angular coordinates. The error vector is:

\[e(q) := \log(T_{tf}) = \log(T_{rt}^{-1} T_{rf})\]

where \(b\) denotes our frame, \(t\) the target frame and \(0\) the inertial frame. See also pink.tasks.task.Task.compute_error() for more context.

Parameters:

configuration (Configuration) – Robot configuration \(q\).

Return type:

ndarray

Returns:

Frame task error \(e(q)\).

compute_jacobian(configuration)

Compute the frame task Jacobian.

The task Jacobian \(J(q) \in \mathbb{R}^{6 \times n_v}\) is the derivative of the task error \(e(q) \in \mathbb{R}^6\) with respect to the configuration \(q\). The formula for the frame task is:

\[J(q) = \text{Jlog}(T_{tf}) ( {}_f J_{0f}(q) - \text{Ad}_{T_{fr}}{}_r J_{0r}(q) )\]

The proof is as follow if we denote \(JT(q)\) the Jacobian in the local frame \(T_{tf}(q) = T_{rt}^{-1} T_{0r}^{-1}(q) T_{0f}(q)\) we have

\[\begin{split}\begin{align} J(q) = \text{Jlog}(T_{tf}) JT [Jv]^{up} & = T_{tf}^{-1} \dot{T}_{tf} \\ & = T_{ft}T_{tr}T_{r0}\dot{T}_{0f} - T_{ft}T_{tr}T_{0r}^{-1}\dot{T}_{0r}T_{0r}^{-1}T_{0f} \\ & = T_{0f}^{-1}\dot{T}_{0f} - T_{fr}(T_{0r}^{-1}\dot{T}_{0r})T_{fr}^{-1} \\ & = [{}_f J_{0f} v - \text{Ad}_{T_{fr}}{}_r J_{0r} v]^{up} \\ J(q) & = {}_f J_{0f} - \text{Ad}_{T_{fr}}{}_r J_{0r} \end{align}\end{split}\]

The formula implemented here is more general than the one detailed in [FrameTaskJacobian]. See also pink.tasks.task.Task.compute_jacobian() for more context on task Jacobians.

Parameters:

configuration (Configuration) – Robot configuration \(q\).

Return type:

ndarray

Returns:

Jacobian matrix \(J\), expressed locally in the frame.

set_orientation_cost(orientation_cost)

Set a new cost for all 3D orientation coordinates.

Parameters:

orientation_cost (Union[float, Sequence[float], ndarray]) – Contribution of orientation errors to the normalized cost, in \([\mathrm{cost}] / [\mathrm{rad}]\). If this is a vector, the cost is anisotropic and each coordinate corresponds to an axis of the frame.

Return type:

None

set_position_cost(position_cost)

Set a new cost for all 3D position coordinates.

Parameters:

position_cost (Union[float, Sequence[float], ndarray]) – Contribution of position errors to the normalized cost, in \([\mathrm{cost}] / [\mathrm{m}]\). If this is a vector, the cost is anisotropic and each coordinate corresponds to an axis of the frame.

Return type:

None

set_target(transform_target_to_root)

Set task target pose in the root frame.

Parameters:

transform_target_to_root (SE3) – Affine transform matrix (4×4) from the task target frame to the root frame.

Return type:

None

set_target_from_configuration(configuration)

Set task target pose from a robot configuration.

Parameters:

configuration (Configuration) – Robot configuration.

Return type:

None

Joint coupling task

Joint coupling task implementation.

class pink.tasks.joint_coupling_task.JointCouplingTask(joint_names, ratios, cost, configuration, lm_damping=0.0, gain=1.0)

Coupling between two revolute joints.

Note

This task only considers a 1-Dimensional task.

Linear holonomic task

Linear holonomic task \(A (q \ominus q_0) = b\).

class pink.tasks.linear_holonomic_task.LinearHolonomicTask(A, b, q_0, cost=None, lm_damping=0.0, gain=1.0)

Linear holonomic task \(A (q \ominus q_0) = b\).

A

Matrix that defines the task:

\begin{align} e(q) & = A (q \ominus^\ell q_0) - b \\ \dot{e}(q) & := A \dot{q} \end{align}

where \(e(q) \in \mathbb{R}^{k}\) is the quantity that the task aims to drive to zero (\(k\) is the dimension of the task).

b

Vector that defines the affine part of the task

q_0

Element for which we work in the Lie algebra. If set to None, it will use the neutral configuration of the robot.

Notes

To be fully explicit, our quantities in the task equation \(e(q) = A (q \ominus^\ell q_0) - b\) belong to the following sets:

\begin{align} e(q) & \in \mathbb{R}^p & A & \in \mathcal{L}(\mathfrak{g}^\ell_{q_0}, \mathbb{R}^p) & b & \in \mathbb{R}^p \end{align}

where \(\mathfrak{g}^\ell_{q_0}\) is the Lie algebra associated with the Lie group \(\mathcal{C}\) (our configuration space), taken in the local frame (\(\ell\), a.k.a. body frame) at the reference configuration \(q_0\).

We take the local difference \(q \ominus^\ell q_0\) between our configurations \(q \in \mathcal{C}\) and \(q_0 \in \mathcal{C}\), which is an element of the local Lie algebra in \(\mathfrak{g}^\ell_{q_0}\) at \(q_0\). In Pinocchio the convention is to use local differences, unless specified otherwise (e.g. world-aligned frames). It should not be confused with the general Lie difference \(\ominus\) (that one is never used in practice), nor the spatial difference \(\ominus^s\), which is an element of the spatial Lie algebra \(\mathfrak{g}^s_{q_0}\) associated with \(q_0\):

\begin{align} q \ominus q_0 & \in \mathcal{T}_{q_0} \mathcal{C} \\ q \ominus^\ell q_0 & \in \mathfrak{g}^\ell_{q_0} \\ q \ominus^s q_0 & \in \mathfrak{g}^s_{q_0} \end{align}

On a side note, here’s a look at the dimensions of these elements:

\begin{align} \mathrm{dim}(q) = \mathrm{dim}(q_0) = n_q \\ \mathrm{dim}(q \ominus q_0) = n_q \\ \mathrm{dim}(q \ominus^\ell q_0) = \mathrm{dim}(q \ominus^s q_0) = n_v \end{align}

Finally, we derive the Jacobian of our error \(e(q)\) as follows, denoting by \(a = \log(q_0^{-1} q)\) so that \(q \ominus^\ell q_0 = \log(a)\):

\begin{align} \left.\frac{\partial e(q)}{\partial q} \right|_{\ell} = A \left. \frac{\partial \log}{\partial q} \right|_{\ell}^{\ell} \left. \frac{\partial a}{\partial q} \right|_{\ell} = A \left. \frac{\partial \log}{\partial q} \right|_{\ell} \end{align}

By virtue of the identity \(\left.{}^\ell \frac{\partial a}{\partial q}\right|_{\ell} = I_{\mathfrak{g}}\). (See Yann de Mont-Marin’s future blog for details :p)

compute_error(configuration)

Compute the task error function.

The error function \(e(q) \in \mathbb{R}^{p}\) is the quantity that the task aims to drive to zero (\(p\) is the dimension of the task). See the documentation of pink.tasks.task.Task.compute_error() for details.

Parameters:

configuration (Configuration) – Robot configuration \(q\).

Return type:

ndarray

Returns:

Task error vector \(e(q)\).

compute_jacobian(configuration)

Compute the task Jacobian at a given configuration.

See the documentation of pink.tasks.task.Task.compute_jacobian() for details on the task Jacobian.

Parameters:

configuration (Configuration) – Robot configuration \(q\).

Return type:

ndarray

Returns:

Task Jacobian \(J(q)\).

Posture task

Posture task implementation.

class pink.tasks.posture_task.PostureTask(cost, lm_damping=0.0, gain=1.0)

Regulate joint angles to a desired posture.

A posture is a vector of actuated joint angles. Floating base coordinates are not affected by this task.

target_q

Target vector in the configuration space. If the model has a floating base, then this vector should include floating-base coordinates (although they have no effect on the posture task).

A posture task is typically used for regularization as it has a steady rank. For instance, when Upkie’s legs are stretched and the Jacobian of its contact frames becomes singular, the posture task will drive the knees toward a preferred orientation.

compute_error(configuration)

Compute posture task error.

The posture task error is defined as:

\[e(q) = q^* \ominus q\]

See pink.tasks.task.Task.compute_error() for more context.

Parameters:

configuration (Configuration) – Robot configuration \(q\).

Return type:

ndarray

Returns:

Posture task error \(e(q)\).

compute_jacobian(configuration)

Compute the posture task Jacobian.

The task Jacobian is the identity \(I_{n_v} \in \mathbb{R}^{n_v \times n_v}\), with \(n_v\) the dimension of the robot’s tangent space, so that the task dynamics are:

\[J(q) \Delta q = \Delta q = \alpha (q^* \ominus q)\]

See pink.tasks.task.Task.compute_jacobian() for more context.

Parameters:

configuration (Configuration) – Robot configuration \(q\).

Return type:

ndarray

Returns:

Posture task Jacobian \(J(q)\).

set_target(target_q)

Set target posture.

Parameters:

target_q (ndarray) – Target vector in the configuration space. If the model has a floating base, then this vector should include floating-base coordinates (although they have no effect on the posture task).

Return type:

None

set_target_from_configuration(configuration)

Set target posture from a robot configuration.

Parameters:

configuration (Configuration) – Robot configuration.

Return type:

None

Rolling task

Rolling task implementation.

class pink.tasks.rolling_task.RollingTask(hub_frame, floor_frame, wheel_radius, cost, gain=1.0, lm_damping=0.0)

Roll without slipping on a plane.

Frames used in this task are:

• Floor: inertial frame defining the contact plane the wheel rolls on.

• Hub: frame attached to the hub of the wheel in the robot model.

• Rim: its origin is the projection of the hub origin onto the floor plane. Its orientation is the same as the floor frame.

hub_frame

Name of a frame attached to the hub of the wheel in the robot model.

floor_frame

Name of the inertial frame whose xy-plane defines the contact surface the wheel is rolling onto.

wheel_radius

Radius of the wheel, i.e. distance in meters from the hub to the nearest point on the rim.

Note

See also the similar wheel task in the PlaCo C++ library. It additionally handles omniwheels.

compute_error(configuration)

Compute the rolling task error.

The error is a 3D vector \({}_R e(q)\) of the rim frame \(R\):

\[\begin{split}{}_R e(q) := \begin{bmatrix} 0 \\ 0 \\ z_{\mathit{hub}} - \rho \end{bmatrix}\end{split}\]

The error is zero along the x- and y-axis of the rim frame to roll without slipping (i.e., the velocity of the contact point is zero in the floor plane). The error along the z-axis of the rim frame is chosen so that the task keeps the wheel hub at distance \(\rho\) (wheel radius) from the floor plane.

Parameters:

configuration (Configuration) – Robot configuration \(q\).

Return type:

ndarray

Returns:

Task error \({}_R e(q) \in \mathbb{R}^3\), a translation expressed in the rim frame \(R\).

compute_jacobian(configuration)

Compute the rolling task Jacobian.

Parameters:

configuration (Configuration) – Robot configuration \(q\).

Return type:

ndarray

Returns:

Translation Jacobian matrix \({}_R J_{WR}\), expressed locally in the rim frame \(R\).