Tasks define the objective functions in an inverse kinematics problem. Each task represents a mapping from the configuration space to a task space, with the solver minimizing the weighted error between the current and desired task values. This approach follows the task-priority framework established in robotics literature [Kanoun et al., 2011], where multiple objectives are managed through appropriate weighting and prioritization.
Mathematically, a task is defined as a function \(f: \mathcal{Q} \rightarrow \mathbb{R}^m\) that maps from the configuration space \(\mathcal{Q}\) to a task space. The error is computed as \(e(q) = f(q) - f_{desired}\), and the solver minimizes a weighted norm of this error: \(\|e(q)\|^2_W\), where \(W\) is a positive-definite weight matrix.
All tasks inherit from the base Task class and follow a consistent mathematical formulation, enabling systematic composition of complex behaviors through the combination of elementary tasks.
Base Task
The foundational class that all tasks extend. It defines the core interface and mathematical properties for task objectives.
- class JaxTask(dim, model, vector_gain, gain_fn, mask_idxs, matrix_cost, lm_damping)
A JAX-based implementation of a task for inverse kinematics.
This class serves as a base for all tasks in the inverse kinematics problem.
Tasks are components that represent goals or objectives for the robot’s configuration. Mathematically, a task defines a function:
\[e(q, t) = f(q, t) - f_{desired}(t)\]- where:
\(e(q, t)\) is the error between current and desired states
\(f(q, t)\) is the current value of the task function
\(f_{desired}(t)\) is the desired value (target) of the task function
The goal of optimization is typically to minimize this error or drive it to zero.
- Parameters:
matrix_cost (
Array) – The cost matrix associated with the task.lm_damping (
float) – The Levenberg-Marquardt damping factor.
- compute_error(data)
Compute the error for the task.
This method is equivalent to calling the task object directly and calculates the error \(e(q, t)\) between the current state and the desired state.
- Parameters:
data (
Data) – The MuJoCo simulation data.- Return type:
- Returns:
The error vector for the task.
- class Task(name, cost, gain, gain_fn=None, lm_damping=0, mask=None)
A high-level representation of a task for inverse kinematics.
This class provides an interface for creating and manipulating tasks in the inverse kinematics problem. The optimization cost associated with a task can be formulated as:
\[C(q, t) = e(q, t)^T W e(q, t)\]- where:
\(e(q, t)\) is the task error vector
\(W\) is the cost weight matrix defined by the matrix_cost property
- Parameters:
name (
str) – The name of the task.cost (
ndarray|Array|float) – The cost associated with the task.gain_fn (
Callable[[float],float] |None) – A function to compute the gain dynamically.lm_damping (
float) – The Levenberg-Marquardt damping factor.mask (
Sequence[int] |None) – A sequence of integers to mask certain dimensions of the task.
- update_cost(cost)
Update the cost for the task.
This method allows setting the cost using either a scalar, vector, or matrix.
- property matrix_cost: Array
Get the cost matrix associated with the task.
- This method converts cost to matrix form as follows:
cost is scalar -> matrix_cost = jnp.eye(self.dim) * cost
cost is vector -> matrix_cost = jnp.diag(cost)
cost is matrix -> matrix_cost = cost
- Returns:
The cost matrix as a numpy array.
- Raises:
ValueError – If the dimension is not set or the cost size is invalid.
Center of Mass Task
Controls the position of the robot’s center of mass, critical for maintaining balance in legged systems and manipulators.
Center of mass task implementation.
- class JaxComTask(dim, model, vector_gain, gain_fn, mask_idxs, matrix_cost, lm_damping, target_com)
A JAX-based implementation of a center of mass (CoM) task for inverse kinematics.
This class represents a task that aims to achieve a specific target center of mass for the robot model.
The task function maps joint positions to the robot’s center of mass position:
\[f(q) = \frac{\sum_i m_i p_i(q)}{\sum_i m_i}\]- where:
\(m_i\) is the mass of body i
\(p_i(q)\) is the position of body i’s center of mass
The error is computed as the difference between the current and target CoM positions:
\[e(q) = p_c(q) - p_{c_{target}}\]- Parameters:
target_com (
Array) – The target center of mass position to be achieved.
- final compute_jacobian(data)
Compute the Jacobian of the component with respect to the joint positions. The jacobian is a matrix \(J \in R^{dim \times nv}\) that is defined as:
\[J(q, t) = \frac{\partial f}{\partial q}\]The main property that is desirable for us is that given this matrix, we obtain component derivative as a linear matrix w.r.t. velocities:
\[\dot{f}(q, t) = \frac{\partial f}{\partial q} v = J(q, t)\]The jacobian is calculated via automatic differentiation, if it is not overwritten. If \(nq \\neq nv\), a special mapping is computed to transform \(J_{dq}\) into \(J_v\). For details, see
mjinx.configuration.jac_dq2v().- Parameters:
data – The MuJoCo simulation data.
- Returns:
The computed Jacobian matrix.
- class ComTask(name, cost, gain, gain_fn=None, lm_damping=0, mask=None)
A high-level representation of a center of mass (CoM) task for inverse kinematics.
This class provides an interface for creating and manipulating center of mass tasks, which aim to achieve a specific target center of mass for the robot model.
- Parameters:
name (
str) – The name of the task.cost (
ndarray|Array|float) – The cost associated with the task.gain_fn (
Callable[[float],float] |None) – A function to compute the gain dynamically.lm_damping (
float) – The Levenberg-Marquardt damping factor.mask (
Sequence[int] |None) – A sequence of integers to mask certain dimensions of the task.
- JaxComponentType
alias of
JaxComTask
- property target_com: Array
Get the current target center of mass for the task.
- Returns:
The current target center of mass as a numpy array.
- update_target_com(target_com)
Update the target center of mass for the task.
This method allows setting the target center of mass using a sequence of values.
- Parameters:
target_com (
Sequence) – The new target center of mass as a sequence of values.- Raises:
ValueError – If the provided sequence doesn’t have the correct length.
Joint Task
Directly controls joint positions, useful for regularization, posture optimization, and redundancy resolution.
Center of mass task implementation.
- class JaxJointTask(dim, model, vector_gain, gain_fn, mask_idxs, matrix_cost, lm_damping, target_q, qmask_idxs)
A JAX-based implementation of a joint task for inverse kinematics.
This class represents a task that aims to achieve specific target joint positions for the robot model.
The task function is the identity map on the joint space:
\[f(q) = q\]The error is computed as the difference between the current and target joint positions:
\[e(q) = q - q_{target}\]For robots with quaternion joints or a floating base, a more sophisticated difference function is used to properly handle the joint topology.
- Parameters:
target_q (
Array) – The full target joint positions vector for all joints in the system.
- compute_jacobian(data)
Compute the Jacobian of the joint task function.
Since the joint task function is the identity map (or a selection of it), the Jacobian is simply an identity matrix or a selection matrix, depending on the mask and whether the system has a floating base:
\[J = I \quad \text{or} \quad J = [0 \; I]\]- Parameters:
data (
Data) – The MuJoCo simulation data.- Return type:
- Returns:
The Jacobian matrix of the barrier function.
- class JointTask(name, cost, gain, gain_fn=None, lm_damping=0, mask=None)
A high-level representation of a joint task for inverse kinematics.
This class provides an interface for creating and manipulating joint tasks, which aim to achieve specific target joint positions for the robot model.
- Parameters:
name (
str) – The name of the task.cost (
ndarray|Array|float) – The cost associated with the task.gain_fn (
Callable[[float],float] |None) – A function to compute the gain dynamically.lm_damping (
float) – The Levenberg-Marquardt damping factor.mask (
Sequence[int] |None) – A sequence of integers to mask certain dimensions of the task.floating_base – A boolean indicating whether the robot has a floating base.
- JaxComponentType
alias of
JaxJointTask
- update_model(model)
Update the MuJoCo model and set the joint dimensions for the task.
This method is called when the model is updated or when the task is first added to the problem. It adjusts the dimensions based on whether the robot has a floating base and validates the mask.
- Parameters:
model (
Model) – The new MuJoCo model.- Raises:
ValueError – If the provided mask is invalid for the model or if the target_q is incompatible.
- property target_q: Array
Get the current target joint positions for the task.
- Returns:
The current target joint positions as a numpy array.
- Raises:
ValueError – If the target value was not provided and the model is missing.
- update_target_q(target_q)
Update the target joint positions for the task.
This method allows setting the target joint positions using a sequence of values.
- Parameters:
target_q (
Sequence) – The new target joint positions as a sequence of values.- Raises:
ValueError – If the provided sequence doesn’t have the correct length.
Base Body Task
The foundation for tasks that target specific bodies, geometries, or sites in the robot model. This abstract class provides common functionality for all object-specific tasks.
Frame task implementation.
- class JaxObjTask(dim, model, vector_gain, gain_fn, mask_idxs, matrix_cost, lm_damping, obj_id, obj_type)
A JAX-based implementation of an object-specific task for inverse kinematics.
This class serves as a base for tasks that are applied to specific objects (bodies, geometries, or sites) in the robot model. The task function relates to the object’s state in the world frame.
Depending on the derived class, the task function can target various aspects of the object’s state, such as position, orientation, or the complete pose.
- Parameters:
obj_id (
int) – The ID of the object (body, geometry, or site) to which the task is applied.obj_type (
mjtObj) – The type of the object (mjOBJ_BODY, mjOBJ_GEOM, or mjOBJ_SITE).
- get_pos(data)
Get the position of the object in the world frame.
This method returns the position based on the object type (body, geom, or site).
- Parameters:
data (
Data) – The MuJoCo simulation data.- Return type:
- Returns:
A 3D vector representing the object’s position in the world frame.
- get_rotation(data)
Get the rotation of the object in the world frame.
This method returns the rotation based on the object type (body, geom, or site).
- Parameters:
data (
Data) – The MuJoCo simulation data.- Return type:
- Returns:
An SO3 object representing the object’s rotation in the world frame.
- get_frame(data)
Get the full pose (position and rotation) of the object in the world frame.
This method combines the position and rotation to return a complete pose.
- Parameters:
data (
Data) – The MuJoCo simulation data.- Return type:
- Returns:
An SE3 object representing the object’s pose in the world frame.
- class ObjTask(name, cost, gain, obj_name, obj_type=<mjtObj.mjOBJ_BODY: 1>, gain_fn=None, lm_damping=0, mask=None)
A high-level representation of an object task for inverse kinematics.
This class provides an interface for creating and manipulating tasks that are applied to specific objects (bodies, geometries, or sites) in the robot model.
Object tasks generally define a function \(f(q, t)\) that maps joint positions \(q\) to some property of the specified object (such as position, orientation, or pose). The error is then calculated as the difference between the current and desired states:
\[e(q, t) = f(q, t) - f_{desired}(t)\]- Parameters:
name (
str) – The name of the task.cost (
ndarray|Array|float) – The cost associated with the task.obj_name (
str) – The name of the object (body, geometry, or site) to which the task is applied.obj_type (
mjtObj) – The type of the object (mjOBJ_BODY, mjOBJ_GEOM, or mjOBJ_SITE).gain_fn (
Callable[[float],float] |None) – A function to compute the gain dynamically.lm_damping (
float) – The Levenberg-Marquardt damping factor.mask (
Sequence[int] |None) – A sequence of integers to mask certain dimensions of the task.
- JaxComponentType
alias of
JaxObjTask
- property obj_name: str
Get the name of the object to which the task is applied.
- Returns:
The name of the object.
- property obj_id: int
Get the ID of the object to which the task is applied.
- Returns:
The ID of the object.
- property obj_type: mjtObj
Get the type of the object associated with the task.
- Returns:
The MuJoCo object type (mjOBJ_BODY, mjOBJ_GEOM, or mjOBJ_SITE).
- update_model(model)
Update the MuJoCo model and set the object ID for the task.
This method is called when the model is updated or when the task is first added to the problem.
- Parameters:
model (
Model) – The new MuJoCo model.- Raises:
ValueError – If the object with the specified name is not found in the model.
Body Frame Task
Controls the complete pose (position and orientation) of a body, geometry, or site using SE(3) representations.
Frame task implementation.
- class JaxFrameTask(dim, model, vector_gain, gain_fn, mask_idxs, matrix_cost, lm_damping, obj_id, obj_type, target_frame)
A JAX-based implementation of a frame task for inverse kinematics.
This class represents a task that aims to achieve a specific target frame for a given object in the robot model.
The task function maps joint positions to the object’s frame (pose) in the world frame:
\[f(q) = T(q) \in SE(3)\]where \(T(q)\) is the transformation matrix representing the object’s pose.
The error is computed using the logarithmic map in SE(3), which represents the relative transformation between current and target frames as a twist vector:
\[e(q) = \log(T(q)^{-1} T_{target})\]This formulation provides a natural way to interpolate between frames and control both position and orientation simultaneously.
- Parameters:
target_frame (
SE3) – The target frame to be achieved.
- final compute_jacobian(data)
Compute the Jacobian of the frame task.
The Jacobian relates changes in joint positions to changes in the frame task error. It is computed as:
\[J = -J_{\log} \cdot J_{frame}^T\]- where:
\(J_{\log}\) is the Jacobian of the logarithmic map at the relative transformation
\(J_{frame}\) is the geometric Jacobian of the object’s frame
- Parameters:
data (
Data) – The MuJoCo simulation data.- Return type:
- Returns:
The Jacobian matrix of the frame task.
- class FrameTask(name, cost, gain, obj_name, obj_type=<mjtObj.mjOBJ_BODY: 1>, gain_fn=None, lm_damping=0, mask=None)
A high-level representation of a frame task for inverse kinematics.
This class provides an interface for creating and manipulating frame tasks, which aim to achieve a specific target frame for a object (body, geom, or site) in the robot model.
- Parameters:
name (
str) – The name of the task.cost (
ndarray|Array|float) – The cost associated with the task.obj_name (
str) – The name of the object to which the task is applied.gain_fn (
Callable[[float],float] |None) – A function to compute the gain dynamically.lm_damping (
float) – The Levenberg-Marquardt damping factor.mask (
Sequence[int] |None) – A sequence of integers to mask certain dimensions of the task.
- JaxComponentType
alias of
JaxFrameTask
- property target_frame: SE3
Get the current target frame for the task.
- Returns:
The current target frame as an SE3 object.
- update_target_frame(target_frame)
Update the target frame for the task.
This method allows setting the target frame using either an SE3 object or a sequence of values representing the frame.
Body Position Task
Controls just the position of a body, geometry, or site, ignoring orientation. Useful when only positional constraints matter.
Frame task implementation.
- class JaxPositionTask(dim, model, vector_gain, gain_fn, mask_idxs, matrix_cost, lm_damping, obj_id, obj_type, target_pos)
A JAX-based implementation of a position task for inverse kinematics.
This class represents a task that aims to achieve a specific target position for an object (body, geometry, or site) in the robot model.
The task function maps joint positions to the object’s position in the world frame:
\[f(q) = p(q)\]where \(p(q)\) is the position of the object in world coordinates.
The error is computed as the difference between the current and target positions:
\[e(q) = p(q) - p_{target}\]The Jacobian of this task is the object’s position Jacobian, which relates changes in joint positions to changes in the object’s position.
- Parameters:
target_pos (
Array) – The target position to be achieved.
- class PositionTask(name, cost, gain, obj_name, obj_type=<mjtObj.mjOBJ_BODY: 1>, gain_fn=None, lm_damping=0, mask=None)
A high-level representation of a position task for inverse kinematics.
This class provides an interface for creating and manipulating position tasks, which aim to achieve a specific target position for an object in the robot model.
- Parameters:
name (
str) – The name of the task.cost (
ndarray|Array|float) – The cost associated with the task.obj_name (
str) – The name of the object to which the task is applied.gain_fn (
Callable[[float],float] |None) – A function to compute the gain dynamically.lm_damping (
float) – The Levenberg-Marquardt damping factor.mask (
Sequence[int] |None) – A sequence of integers to mask certain dimensions of the task.
- JaxComponentType
alias of
JaxPositionTask
- property target_pos: Array
Get the current target position for the task.
- Returns:
The current target position as a numpy array.
- update_target_pos(target_pos)
Update the target position for the task.
This method allows setting the target position using a sequence of values.
- Parameters:
target_pos (
Sequence) – The new target position as a sequence of values.- Raises:
ValueError – If the provided sequence doesn’t have the correct length.