Tasks

Tasks#

All kinematic tasks derive from the Task base class.

class mink.tasks.task.Objective#

Quadratic objective of the form \(\frac{1}{2} \Delta q^T H \Delta q + c^T \Delta q\).

H: ndarray#: Hessian matrix, of shape (\(n_v\), \(n_v\)).

c: ndarray#: Linear vector, of shape (\(n_v\),).

value(x: ndarray) → float#

Returns the value of the objective at the input vector.

Parameters:: x (ndarray)
Return type:: float

class mink.tasks.task.BaseTask#

Base class for all tasks.

abstract compute_qp_objective(configuration: Configuration) → Objective#

Compute the matrix-vector pair \((H, c)\) of the QP objective.

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Returns:: Pair \((H(q), c(q))\).
Return type:: Objective

class mink.tasks.task.Task#

Abstract base class for kinematic tasks.

Subclasses must implement the configuration-dependent task error compute_error() and Jacobian compute_jacobian() functions.

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 the derivative of the task error \(e(q)\) with respect to the configuration \(q \in \mathbb{R}^{n_q}\). The configuration displacement \(\Delta q\) is the output of inverse kinematics; we divide it by dt to get a commanded velocity.

In the first-order task dynamics, the error \(e(q)\) is multiplied by the task gain \(\alpha \in [0, 1]\). This gain can be 1.0 for dead-beat control (i.e. converge as fast as possible), but might be unstable as it neglects our first-order approximation. Lower values cause slow down the task, similar to low-pass filtering.

abstract compute_error(configuration: Configuration) → ndarray#

Compute the task error at the current configuration.

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Returns:: Task error vector \(e(q)\).
Return type:: ndarray

abstract compute_jacobian(configuration: Configuration) → ndarray#

Compute the task Jacobian at the current configuration.

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Returns:: Task jacobian \(J(q)\).
Return type:: ndarray

compute_qp_objective(configuration: Configuration) → Objective#

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 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}\) weights and normalizes task coordinates to the same unit. The unit of the overall contribution is [cost]^2.

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Returns:: Pair \((H(q), c(q))\).
Return type:: Objective

Kinematic Tasks#

Frame task#

Frame task implementation.

class mink.tasks.frame_task.FrameTask#

Regulate the position and orientation of a frame of interest on the robot.

frame_name#: Name of the frame to regulate, typically the name of body, geom or site in the robot model.

frame_type#: The frame type: body, geom or site.

transform_target_to_world#

Target pose of the frame in the world frame.

Type:: mink.lie.se3.SE3 | None

Example:

frame_task = FrameTask(
    frame_name="target",
    frame_type="site",
    position_cost=1.0,
    orientation_cost=1.0,
)

# Update the target pose directly.
transform_target_to_world = SE3.from_translation(np.random.rand(3))
frame_task.set_target(transform_target_to_world)

# Or from the current configuration. This will automatically compute the
# target pose from the current configuration and update the task target.
frame_task.set_target_from_configuration(configuration)

set_target(transform_target_to_world: SE3) → None#

Set the target pose.

Parameters:: transform_target_to_world (SE3) – Transform from the task target frame to the world frame.
Return type:: None

set_target_from_configuration(configuration: Configuration) → None#

Set the target pose from a given robot configuration.

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

compute_error(configuration: Configuration) → ndarray#

Compute the frame task error.

This error is a twist \(e(q) \in se(3)\) expressed in the local frame, i.e., it is a body twist. It is computed by taking the right-minus difference between the target pose \(T_{0t}\) and current frame pose \(T_{0b}\):

\[e(q) := {}_b \xi_{0b} = -(T_{t0} \ominus T_{b0}) = -\log(T_{t0} \cdot T_{0b}) = -\log(T_{tb}) = \log(T_{bt})\]

where \(b\) denotes our frame, \(t\) the target frame and \(0\) the inertial frame.

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Returns:: Frame task error vector \(e(q)\).
Return type:: ndarray

compute_jacobian(configuration: Configuration) → ndarray#

Compute the frame task Jacobian.

The derivation of the formula for this Jacobian is detailed in [FrameTaskJacobian].

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Returns:: Frame task jacobian \(J(q)\).
Return type:: ndarray

compute_qp_objective(configuration: Configuration) → Objective#

Compute the matrix-vector pair \((H, c)\) of the QP objective.

Overrides the base implementation to compute the frame transform once and reuse it for both error and Jacobian computation.

Parameters:: configuration (Configuration)
Return type:: Objective

Relative frame task#

Relative frame task implementation.

class mink.tasks.relative_frame_task.RelativeFrameTask#

Regulate the pose of a frame relative to another frame.

frame_name#: Name of the frame to regulate, typically the name of body, geom or site in the robot model.

frame_type#: The frame type: body, geom or site.

root_name#: Name of the frame the task is relative to.

root_type#: The root frame type: body, geom or site.

transform_target_to_root#

Target pose in the root frame.

Type:: mink.lie.se3.SE3 | None

set_target(transform_target_to_root: SE3) → None#

Set the target pose in the root frame.

Parameters:: transform_target_to_root (SE3) – Transform from the task target frame to the root frame.
Return type:: None

set_target_from_configuration(configuration: Configuration) → None#

Set the target pose from a given robot configuration.

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

compute_error(configuration: Configuration) → ndarray#

Compute the task error at the current configuration.

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Returns:: Task error vector \(e(q)\).
Return type:: ndarray

compute_jacobian(configuration: Configuration) → ndarray#

Compute the task Jacobian at the current configuration.

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Returns:: Task jacobian \(J(q)\).
Return type:: ndarray

compute_qp_objective(configuration: Configuration) → Objective#

Compute the matrix-vector pair \((H, c)\) of the QP objective.

Overrides the base implementation to compute shared quantities once.

Parameters:: configuration (Configuration)
Return type:: Objective

Center of mass task#

Center-of-mass task implementation.

class mink.tasks.com_task.ComTask#

Regulate the center-of-mass (CoM) of a robot.

target_com#

Target position of the CoM.

Type:: numpy.ndarray | None

Example:

com_task = ComTask(model, cost=1.0)

# Update the target CoM directly.
com_desired = np.zeros(3)
com_task.set_target(com_desired)

# Or from a keyframe defined in the model.
configuration.update_from_keyframe("home")
com_task.set_target_from_configuration(configuration)

set_cost(cost: ArrayLike) → None#

Set the cost of the CoM task.

Parameters:: cost (ArrayLike) – A vector of shape (1,) (aka identical cost for all coordinates), or (3,) (aka different costs for each coordinate).
Return type:: None

set_target(target_com: ArrayLike) → None#

Set the target CoM position in the world frame.

Parameters:: target_com (ArrayLike) – A vector of shape (3,) representing the desired center-of-mass position in the world frame.
Return type:: None

set_target_from_configuration(configuration: Configuration) → None#

Set the target CoM from a given robot configuration.

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

compute_error(configuration: Configuration) → ndarray#

Compute the CoM task error.

The center of mass \(c(q)\) for a collection of bodies \(\mathcal{B}\) is the mass-weighted average of their individual centers of mass. After running forward kinematics, in particular after calling mj_comPos, MuJoCo stores the CoM of each subtree in data.subtree_com. This task uses the CoM of the subtree starting from body 1, which is the entire robot excluding the world body (body 0).

The task error \(e(q)\) is the difference between the current CoM \(c(q)\) and the target CoM \(c^*\):

\[e(q) = c(q) - c^*\]

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Returns:: Center-of-mass task error vector \(e(q)\).
Return type:: ndarray

compute_jacobian(configuration: Configuration) → ndarray#

Compute the Jacobian of the CoM task error \(e(q)\).

The Jacobian is the derivative of this error with respect to the generalized coordinates \(q\). Since the target \(c^*\) is constant, the Jacobian of the error simplifies to the Jacobian of the CoM position \(c(q)\):

\[J(q) = \frac{\partial e(q)}{\partial q} = \frac{\partial c(q)}{\partial q}\]

MuJoCo’s mj_jacSubtreeCom function computes this Jacobian using the formula:

\[\frac{\partial c(q)}{\partial q} = \frac{1}{M} \sum_{i \in \mathcal{B}} m_i \frac{\partial p_i(q)}{\partial q} = \frac{1}{M} \sum_{i \in \mathcal{B}} m_i J_i(q)\]

where \(M = \sum_{i \in \mathcal{B}} m_i\) is the total mass of the subtree, \(m_i\) is the mass of body \(i\), \(p_i(q)\) is the position of the origin of body frame \(i\) in the world frame, \(J_i(q) = \frac{\partial p_i(q)}{\partial q}\) is the Jacobian mapping joint velocities to the linear velocity of the origin of body frame \(i\), and the sum is over all bodies \(\mathcal{B}\) in the specified subtree (body 1 and its descendants).

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Returns:: Jacobian of the center-of-mass task error \(J(q)\).
Return type:: ndarray

Equality constraint task#

Equality constraint task implementation.

class mink.tasks.equality_constraint_task.EqualityConstraintTask#

Regulate equality constraints in a model.

Equality constraints are useful, among other things, for modeling “loop joints” such as four-bar linkages. In MuJoCo, there are several types of equality constraints, including:

mjEQ_CONNECT: Connect two bodies at a point (ball joint).
mjEQ_WELD: Fix relative pose of two bodies.
mjEQ_JOINT: Couple the values of two scalar joints.
mjEQ_TENDON: Couple the values of two tendons.

This task can regulate all equality constraints in the model or a specific subset identified by name or ID.

Note

MuJoCo computes the constraint residual and its Jacobian and stores them in data.efc_pos and data.efc_J (potentially in sparse format), respectively. The compute_error() and compute_jacobian() methods simply extract the rows corresponding to the active equality constraints specified for this task from data.efc_pos and data.efc_J. More information on MuJoCo’s constraint model can be found in [MuJoCoEqualityConstraints].

equalities#: ID or name of the equality constraints to regulate. If not provided, the task will regulate all equality constraints in the model.

cost#: Cost vector for the equality constraint task. Either a scalar, in which case the same cost is applied to all constraints, or a vector of shape (neq,), where neq is the number of equality constraints in the model.

Raises:

InvalidConstraint – If a specified equality constraint name or ID is not found, or if the constraint is not active at the initial configuration.
TaskDefinitionError – If no equality constraints are found or if cost parameters have invalid shape or values.

Example:

# Regulate all equality constraints with the same cost.
eq_task = EqualityConstraintTask(model, cost=1.0)

# Regulate specific equality constraints with different costs.
eq_task = EqualityConstraintTask(
    model,
    cost=[1.0, 0.5],
    equalities=["connect_right", "connect_left"]
)

set_cost(cost: ArrayLike) → None#

Set the cost vector for the equality constraint task.

Parameters:: cost (ArrayLike) – Cost vector for the equality constraint task.
Return type:: None

compute_error(configuration: Configuration) → ndarray#

Compute the task error (constraint residual) \(e(q) = r(q)\).

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Returns:: Task error vector \(e(q)\) for the active equality constraints.
Return type:: ndarray

compute_jacobian(configuration: Configuration) → ndarray#

Compute the task Jacobian (constraint Jacobian) \(J(q) = J_r(q)\).

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Returns:: Task jacobian \(J(q)\) for the active equality constraints.
Return type:: ndarray

DOF freezing task#

DOF freezing task implementation.

class mink.tasks.dof_freezing_task.DofFreezingTask#

Freeze specific degrees of freedom to zero velocity.

This task is typically used as an equality constraint to prevent specific joints from moving. It enforces zero velocity on the selected DOFs.

dof_indices#: List of DOF indices to freeze (zero velocity).

Example:

# Freeze specific DOFs by index.
dof_freezing_task = DofFreezingTask(
    model=model,
    dof_indices=[0, 1, 2]  # Freeze first 3 DOFs
)

# Use as equality constraint in IK solver.
v = solve_ik(
    configuration=configuration,
    tasks=[frame_task, com_task],
    constraints=[dof_freezing_task],  # Enforce exactly
    dt=dt,
    solver="proxqp",
)

# Freeze specific joints by name.
joint_names = ["shoulder_pan", "shoulder_lift"]
dof_indices = []
for joint_name in joint_names:
    joint_id = model.joint(joint_name).id
    dof_adr = model.jnt_dofadr[joint_id]
    dof_indices.append(dof_adr)

dof_freezing_task = DofFreezingTask(model=model, dof_indices=dof_indices)

compute_error(configuration: Configuration) → ndarray#

Compute the DOF freezing task error.

The error is always zero since we’re constraining velocity, not position. When used as an equality constraint with zero error, this enforces Δq[dof] = 0 for each frozen DOF.

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Returns:: Zero vector of shape \((k,)\) where \(k\) is the number of frozen DOFs.
Return type:: ndarray

compute_jacobian(configuration: Configuration) → ndarray#

Compute the DOF freezing task Jacobian.

The Jacobian is a matrix with one row per frozen DOF, where each row is a standard basis vector (row of the identity matrix) selecting the corresponding DOF.

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Returns:: Jacobian matrix of shape \((k, n_v)\) where \(k\) is the number of frozen DOFs and \(n_v\) is the number of velocity DOFs.
Return type:: ndarray

Regularization Tasks#

Posture task#

Posture task implementation.

class mink.tasks.posture_task.PostureTask#

Regulate joint angles towards a target posture.

Often used with a low priority in the task stack, this task acts like a regularizer, biasing the solution toward a specific joint configuration.

target_q#

Target configuration \(q^*\), of shape \((n_q,)\). Units are radians for revolute joints and meters for prismatic joints. Note that floating-base coordinates are not affected by this task but should be included in the target configuration.

Type:: numpy.ndarray | None

Example:

posture_task = PostureTask(model, cost=1e-3)

# Update the target posture directly.
q_desired = ...
posture_task.set_target(q_desired)

# Or from a keyframe defined in the model.
configuration.update_from_keyframe("home")
posture_task.set_target_from_configuration(configuration)

set_target(target_q: ArrayLike) → None#

Set the target posture.

Parameters:: target_q (ArrayLike) – A vector of shape (nq,) representing the desired joint configuration.
Return type:: None

set_target_from_configuration(configuration: Configuration) → None#

Set the target posture by extracting it from the current configuration.

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

compute_error(configuration: Configuration) → ndarray#

Compute the posture task error.

The error is defined as:

\[e(q) = q \ominus q^*\]

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Returns:: Posture task error vector \(e(q)\).
Return type:: ndarray

compute_jacobian(configuration: Configuration) → ndarray#

Compute the posture task Jacobian.

The task Jacobian is defined as:

\[J(q) = I_{n_v}\]

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Returns:: Posture task jacobian \(J(q)\).
Return type:: ndarray

Damping task#

Damping task implementation.

class mink.tasks.damping_task.DampingTask#

L2-regularization on joint velocities (a.k.a. velocity damping).

This task, typically used with a low priority in the task stack, adds a Levenberg-Marquardt term to the quadratic program, favoring minimum-norm joint velocities in redundant or near-singular situations. Formally, it contributes the following term to the quadratic program:

\[\frac{1}{2}\,\Delta \mathbf{q}^\top \Lambda^2\,\Delta \mathbf{q} = \frac{1}{2}\sum_{i=1}^{n_v} \lambda_i^2 \,(\Delta q_i)^2,\]

where \(\Delta \mathbf{q}\in\mathbb{R}^{n_v}\) is the vector of joint displacements and \(\lambda_i\) is the i-th element of the cost parameter. The quadratic form uses \(\Lambda^2 = \mathrm{diag}(\lambda_1^2, \ldots, \lambda_{n_v}^2)\). A larger \(\lambda_i\) reduces motion in DoF \(i\). With no other active tasks, the robot remains at rest. Unlike the damping parameter in solve_ik(), which is uniformly applied to all DoFs, this task does not affect the floating-base coordinates.

Note

This task does not favor a particular posture, only small instantaneous motion. If you need a posture bias, use PostureTask instead.

Example:

# Uniform damping across all degrees of freedom.
damping_task = DampingTask(model, cost=1.0)

# Custom damping.
cost = np.zeros(model.nv)
cost[:3] = 1.0  # High damping for the first 3 joints.
cost[3:] = 0.1  # Lower damping for the remaining joints.
damping_task = DampingTask(model, cost)

compute_error(configuration: Configuration) → ndarray#

Compute the damping task error.

The damping task does not chase a reference; its desired joint velocity is identically zero, so the task error is always:

\[e = \mathbf 0 \in \mathbb R^{n_v}.\]

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Returns:: Zero vector of length \(n_v\).
Return type:: ndarray

Kinetic energy regularization task#

Kinetic energy regularization task implementation.

class mink.tasks.kinetic_energy_regularization_task.KineticEnergyRegularizationTask#

Kinetic-energy regularization.

This task, often used with a low priority in the task stack, penalizes the system’s kinetic energy. Formally, it contributes the following term to the quadratic program:

\[\frac{1}{2}\, \lambda\, \Delta \mathbf{q}^\top \frac{M(\mathbf{q})}{(\Delta t)^2}\,\Delta \mathbf{q},\]

where \(\Delta \mathbf{q}\in\mathbb{R}^{n_v}\) is the vector of joint displacements, \(M(\mathbf{q})\) is the joint-space inertia matrix, \(\lambda\) is the scalar strength of the regularization, and \(\Delta t\) is the integration timestep.

Note

This task can be seen as an inertia-weighted version of the DampingTask. Degrees of freedom with higher inertia will move less for the same cost.

Warning

The integration timestep \(\Delta t\) must be set via set_dt() before use. This ensures the cost is expressed in units of energy (Joules).

Example:

task = KineticEnergyRegularizationTask(cost=1e-4)
task.set_dt(0.02)

set_dt(dt: float) → None#

Set the integration timestep.

Parameters:: dt (float) – Integration timestep in [s].
Return type:: None

compute_qp_objective(configuration: Configuration) → Objective#

Compute the matrix-vector pair \((H, c)\) of the QP objective.

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Returns:: Pair \((H(q), c(q))\).
Return type:: Objective

Tasks

Contents

Tasks#

Kinematic Tasks#

Frame task#

Relative frame task#

Center of mass task#

Equality constraint task#

DOF freezing task#

Regularization Tasks#

Posture task#

Damping task#

Kinetic energy regularization task#