Tasks¶

Base task¶

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.

Type:: float | Sequence[float] | numpy.ndarray | None

gain¶

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

Type:: float

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.

Type:: float

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 computed by Task.compute_jacobian(), while the configuration displacement \(\\Delta q\) is the output of inverse kinematics. The error vector \(e(q)\) is multiplied by the task gain \(\alpha \in [0, 1]\). The gain is usually 1 for dead-beat control (i.e. converge as fast as possible), but it can also be lower for some extra 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}\) appears in the first-order task dynamics:

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

The error \(e(q) \in \mathbb{R}^{k \times n_v}\), with \(k\) the dimension of the task and \(n_v\) the dimension of the robot’s tangent space, is computed by Task.compute_error(), while the configuration displacement \(\\Delta q\) is the output of inverse kinematics.

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.

Body task¶

Body task implementation.

class pink.tasks.frame_task.FrameTask(body, position_cost, orientation_cost, lm_damping=0.0)¶

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

body¶

Body frame name, typically the link name from the URDF.

Type:: str

transform_target_to_world¶

Target pose for the body frame.

Type:: pinocchio.pinocchio_pywrap.SE3 | None

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 body task error.

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

The error is the right-minus difference between target and current body configuration:

\[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 the body frame, \(t\) the target frame and \(0\) the inertial frame.

See 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:: Body task error \(e(q)\).

compute_jacobian(configuration)¶

Compute the body task Jacobian.

The task Jacobian \(J(q) \in \mathbb{R}^{6 \times n_v}\) appears in the task dynamics:

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

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

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Return type:: ndarray
Returns:: Pair \((J, \alpha e)\) of Jacobian matrix and error vector, both expressed in the body 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 in the local body 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 in the local body 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

Joint coupling task¶

Joint coupling task implementation.

class pink.tasks.joint_coupling_task.JointCouplingTask(joint_names, ratios, cost, configuration, lm_damping=0.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)¶

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

Type:: numpy.ndarray

b¶

Vector that defines the affine part of the task

Type:: numpy.ndarray

q_0¶

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

Type:: numpy.ndarray

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). It appears in the first-order task dynamics:

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

See the description of the task above for details about the calculation of this Jacobian.

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.

The task Jacobian \(J(q) \in \mathbb{R}^{p \times n_v}\) appears in the first-order task dynamics:

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

See the description of the task above for details about the calculation of this 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)¶

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.

Type:: numpy.ndarray | None

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 become singular, the posture task will drive the knees toward a preferred orientation.

compute_error(configuration)¶

Compute posture task error.

See 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 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^* - q)\]

See 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.
Return type:: None

set_target_from_configuration(configuration)¶

Set target posture from a robot configuration.

Parameters:: configuration (Configuration) – Robot configuration.
Return type:: None