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.
See also
Levenberg-Marquardt damping is described in [Sugihara2011]. The dimensional analysis in this class is our own.
- set_orientation_cost(orientation_cost)¶
Set a new cost for all 3D orientation coordinates.
- Parameters:
orientation_cost (
Union
[float
,Sequence
[float
]]) – 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
]]) – 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_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