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.

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)

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.

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

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)

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)

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

Coupling between two revolute joints.

Note

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

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

A

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

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

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)

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)

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