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.
See also
Levenberg-Marquardt damping is described in [Sugihara2011]. The dimensional analysis in this class is our own.
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