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¶

Abstract base class for kinematic tasks.

gain¶

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

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

abstract 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.body_task.BodyTask(body, position_cost, orientation_cost, lm_damping=1e-06)¶

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

body¶

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

Type:: str

cost¶

6D vector that specifies how much each coordinate (in the local body frame) contributes to the cost. Position costs come first (Pinocchio spatial vector convention) and are in \([\mathrm{cost}] / [\mathrm{m}]\), where the the unit of \([\mathrm{cost}]\) up to the user. They are followed by orientation costs in \([\mathrm{cost}] / [\mathrm{rad}]\). Set a cost to zero to disable the task along a coordinate (no cost no effect).

Type:: numpy.ndarray

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

transform_target_to_world¶

Target pose for the body frame.

Type:: pinocchio.pinocchio_pywrap_default.SE3 | None

Costs are designed so that position/orientation costs can be compared between tasks. For example, if task 1 has a position cost of 1.0 and task 2 a position cost of 0.1, then a 1 [cm] error in task 1 costs as much as a 10 [cm] error in task 2.

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 [BodyTaskJacobian]. 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.

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}^{6 \times 6}\) combines position and orientation costs. The unit of the overall contribution is \([\mathrm{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.

Joint coupling task¶

Joint coupling task implementation.

class pink.tasks.joint_coupling_task.JointCouplingTask(joint_name_list, ratios, cost, configuration)¶: Coupling between two revolute joints.

Note

This task only considers a 1-Dimensional task.

Linear holonomic task¶

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

class pink.tasks.linear_holonomic_task.LinearHolonomicTask(A, cost)¶

Linear holonomic task \(A q = 0\).

A¶

matrix that defines the task:

\[e(q) = A q \dot{e}(q) := A \dot{q}\]

where :math: 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

cost¶

joint angular error cost in \([\mathrm{cost}] / [\mathrm{rad}]\).

Type:: float | Sequence[float]

Note

A linear holonomic task is typically used for a robot that has mechanical constraint (e.g., closed loop kinematics). Floating base coordinates are not affected by this task.

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

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}^{n \times n}\) weighs and normalizes task coordinates to the same unit. The unit of the overall contribution is \([\mathrm{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.
Return type:: Tuple[ndarray, ndarray]
Returns:: Pair \((H)\) of Hessian matrix of the QP objective.

Posture task¶

Posture task implementation.

class pink.tasks.posture_task.PostureTask(cost)¶

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.

cost¶

joint angular error cost in \([\mathrm{cost}] / [\mathrm{rad}]\).

Type:: float

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

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}^{n \times n}\) weighs and normalizes task coordinates to the same unit. The unit of the overall contribution is \([\mathrm{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.
Return type:: Tuple[ndarray, ndarray]
Returns:: Pair \((H, c)\) of Hessian matrix and linear vector of the QP objective.

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