mink 0.0.9__py3-none-any.whl → 0.0.11__py3-none-any.whl

mink/__init__.py

@@ -9,11 +9,19 @@ from .constants import (
     SUPPORTED_FRAMES,
 )
 from .exceptions import (
+    IntegrationTimestepNotSet,
+    InvalidConstraint,
+    InvalidDamping,
     InvalidFrame,
+    InvalidGain,
     InvalidKeyframe,
     InvalidMocapBody,
+    InvalidTarget,
+    LimitDefinitionError,
     MinkError,
     NotWithinConfigurationLimits,
+    TargetNotSet,
+    TaskDefinitionError,
     UnsupportedFrame,
 )
 from .lie import SE3, SO3, MatrixLieGroup
@@ -30,10 +38,10 @@ from .tasks import (
     DampingTask,
     EqualityConstraintTask,
     FrameTask,
+    KineticEnergyRegularizationTask,
     Objective,
     PostureTask,
     RelativeFrameTask,
-    TargetNotSet,
     Task,
 )
 from .utils import (
@@ -55,6 +63,7 @@ __all__ = (
     "RelativeFrameTask",
     "PostureTask",
     "Task",
+    "KineticEnergyRegularizationTask",
     "EqualityConstraintTask",
     "Objective",
     "ConfigurationLimit",
@@ -72,6 +81,13 @@ __all__ = (
     "NotWithinConfigurationLimits",
     "TargetNotSet",
     "InvalidMocapBody",
+    "InvalidConstraint",
+    "InvalidDamping",
+    "InvalidGain",
+    "InvalidTarget",
+    "LimitDefinitionError",
+    "TaskDefinitionError",
+    "IntegrationTimestepNotSet",
     "SUPPORTED_FRAMES",
     "FRAME_TO_ENUM",
     "FRAME_TO_JAC_FUNC",

mink/configuration.py

@@ -237,6 +237,21 @@ class Configuration:
         mujoco.mj_integratePos(self.model, self.data.qpos, velocity, dt)
         self.update()
 
+    def get_inertia_matrix(self) -> np.ndarray:
+        r"""Return the joint-space inertia matrix at the current configuration.
+
+        Returns:
+            The joint-space inertia matrix :math:`M(\mathbf{q})`.
+        """
+        # Run the composite rigid body inertia (CRB) algorithm to populate the joint
+        # space inertia matrix data.qM.
+        mujoco.mj_crb(self.model, self.data)
+        # data.qM is stored in a custom sparse format and can be converted to dense
+        # format using mujoco.mj_fullM.
+        M = np.empty((self.nv, self.nv), dtype=np.float64)
+        mujoco.mj_fullM(self.model, M, self.data.qM)
+        return M
+
     # Aliases.
 
     @property

mink/exceptions.py

@@ -96,3 +96,43 @@ class NotWithinConfigurationLimits(MinkError):
             f"{lower} <= {value} <= {upper}"
         )
         super().__init__(message)
+
+
+class TaskDefinitionError(MinkError):
+    """Exception raised when a task definition is ill-formed."""
+
+
+class IntegrationTimestepNotSet(MinkError):
+    """Exception raised when the integration timestep is not set."""
+
+    def __init__(self, cls_name: str):
+        message = f"No integration timestep set for {cls_name}"
+        super().__init__(message)
+
+
+class TargetNotSet(MinkError):
+    """Exception raised when attempting to use a task with an unset target."""
+
+    def __init__(self, cls_name: str):
+        message = f"No target set for {cls_name}"
+        super().__init__(message)
+
+
+class InvalidTarget(MinkError):
+    """Exception raised when the target is invalid."""
+
+
+class InvalidGain(MinkError):
+    """Exception raised when the gain is outside the valid range."""
+
+
+class InvalidDamping(MinkError):
+    """Exception raised when the damping is outside the valid range."""
+
+
+class InvalidConstraint(MinkError):
+    """Exception raised when a constraint is invalid."""
+
+
+class LimitDefinitionError(MinkError):
+    """Exception raised when a limit definition is ill-formed."""

mink/lie/so3.py

@@ -85,6 +85,7 @@ class SO3(MatrixLieGroup):
         assert matrix.shape == (SO3.matrix_dim, SO3.matrix_dim)
         wxyz = np.empty(SO3.parameters_dim, dtype=np.float64)
         mujoco.mju_mat2Quat(wxyz, matrix.ravel())
+        # NOTE mju_mat2Quat normalizes the quaternion.
         return SO3(wxyz=wxyz)
 
     @classmethod
@@ -163,20 +164,22 @@ class SO3(MatrixLieGroup):
     def exp(cls, tangent: np.ndarray) -> SO3:
         axis = np.array(tangent)
         theta = mujoco.mju_normalize3(axis)
-        wxyz = np.zeros(4)
+        wxyz = np.empty(4, dtype=np.float64)
+        # NOTE mju_axisAngle2Quat does not normalize the quaternion but is guaranteed
+        # to return a unit quaternion when axis is a unit vector. In our case,
+        # mju_normalize3 ensures that axis is a unit vector.
         mujoco.mju_axisAngle2Quat(wxyz, axis, theta)
         return SO3(wxyz=wxyz)
 
     # Eq. 133.
     def log(self) -> np.ndarray:
-        if self.wxyz[0] < 0.0:
-            theta = 2.0 * np.arccos(-self.wxyz[0])
-            axis = -1.0 * np.array(self.wxyz[1:])
-        else:
-            theta = 2.0 * np.arccos(self.wxyz[0])
-            axis = np.array(self.wxyz[1:])
-        mujoco.mju_normalize3(axis)
-        return theta * axis
+        q = np.array(self.wxyz)
+        q *= np.sign(q[0])
+        w, v = q[0], q[1:]
+        norm = mujoco.mju_normalize3(v)
+        if norm < get_epsilon(v.dtype):
+            return np.zeros_like(v)
+        return 2 * np.arctan2(norm, w) * v
 
     # Eq. 139.
     def adjoint(self) -> np.ndarray:

mink/limits/__init__.py

@@ -2,7 +2,6 @@
 
 from .collision_avoidance_limit import CollisionAvoidanceLimit
 from .configuration_limit import ConfigurationLimit
-from .exceptions import LimitDefinitionError
 from .limit import Constraint, Limit
 from .velocity_limit import VelocityLimit
 
@@ -12,5 +11,4 @@ __all__ = (
     "Constraint",
     "Limit",
     "VelocityLimit",
-    "LimitDefinitionError",
 )

mink/limits/configuration_limit.py

@@ -7,7 +7,7 @@ import numpy as np
 
 from ..configuration import Configuration
 from ..constants import dof_width, qpos_width
-from .exceptions import LimitDefinitionError
+from ..exceptions import LimitDefinitionError
 from .limit import Constraint, Limit
 
 
@@ -98,7 +98,7 @@ class ConfigurationLimit(Limit):
             return Constraint()
 
         # Upper.
-        delta_q_max = np.zeros((self.model.nv,))
+        delta_q_max = np.empty((self.model.nv,))
         mujoco.mj_differentiatePos(
             m=self.model,
             qvel=delta_q_max,
@@ -108,7 +108,7 @@ class ConfigurationLimit(Limit):
         )
 
         # Lower.
-        delta_q_min = np.zeros((self.model.nv,))
+        delta_q_min = np.empty((self.model.nv,))
         mujoco.mj_differentiatePos(
             m=self.model,
             qvel=delta_q_min,

mink/limits/velocity_limit.py

@@ -8,7 +8,7 @@ import numpy.typing as npt
 
 from ..configuration import Configuration
 from ..constants import dof_width
-from .exceptions import LimitDefinitionError
+from ..exceptions import LimitDefinitionError
 from .limit import Constraint, Limit
 
 

mink/solve_ik.py

@@ -7,11 +7,11 @@ import qpsolvers
 
 from .configuration import Configuration
 from .limits import ConfigurationLimit, Limit
-from .tasks import Objective, Task
+from .tasks import BaseTask, Objective
 
 
 def _compute_qp_objective(
-    configuration: Configuration, tasks: Sequence[Task], damping: float
+    configuration: Configuration, tasks: Sequence[BaseTask], damping: float
 ) -> Objective:
     H = np.eye(configuration.model.nv) * damping
     c = np.zeros(configuration.model.nv)
@@ -42,7 +42,7 @@ def _compute_qp_inequalities(
 
 def build_ik(
     configuration: Configuration,
-    tasks: Sequence[Task],
+    tasks: Sequence[BaseTask],
     dt: float,
     damping: float = 1e-12,
     limits: Optional[Sequence[Limit]] = None,
@@ -53,7 +53,9 @@ def build_ik(
         configuration: Robot configuration.
         tasks: List of kinematic tasks.
         dt: Integration timestep in [s].
-        damping: Levenberg-Marquardt damping.
+        damping: Levenberg-Marquardt damping. Higher values improve numerical
+            stability but slow down task convergence. This value applies to all
+            dofs, including floating-base coordinates.
         limits: List of limits to enforce. Set to empty list to disable. If None,
             defaults to a configuration limit.
 
@@ -67,7 +69,7 @@ def build_ik(
 
 def solve_ik(
     configuration: Configuration,
-    tasks: Sequence[Task],
+    tasks: Sequence[BaseTask],
     dt: float,
     solver: str,
     damping: float = 1e-12,
@@ -85,7 +87,9 @@ def solve_ik(
         tasks: List of kinematic tasks.
         dt: Integration timestep in [s].
         solver: Backend quadratic programming (QP) solver.
-        damping: Levenberg-Marquardt damping.
+        damping: Levenberg-Marquardt damping applied to all tasks. Higher values
+            improve numerical stability but slow down task convergence. This
+            value applies to all dofs, including floating-base coordinates.
         safety_break: If True, stop execution and raise an exception if
             the current configuration is outside limits. If False, print a
             warning and continue execution.

mink/tasks/__init__.py

@@ -3,20 +3,14 @@
 from .com_task import ComTask
 from .damping_task import DampingTask
 from .equality_constraint_task import EqualityConstraintTask
-from .exceptions import (
-    InvalidConstraint,
-    InvalidDamping,
-    InvalidGain,
-    InvalidTarget,
-    TargetNotSet,
-    TaskDefinitionError,
-)
 from .frame_task import FrameTask
+from .kinetic_energy_regularization_task import KineticEnergyRegularizationTask
 from .posture_task import PostureTask
 from .relative_frame_task import RelativeFrameTask
-from .task import Objective, Task
+from .task import BaseTask, Objective, Task
 
 __all__ = (
+    "BaseTask",
     "ComTask",
     "FrameTask",
     "Objective",
@@ -24,11 +18,6 @@ __all__ = (
     "PostureTask",
     "RelativeFrameTask",
     "Task",
-    "TargetNotSet",
-    "InvalidTarget",
-    "TaskDefinitionError",
-    "InvalidConstraint",
-    "InvalidGain",
-    "InvalidDamping",
     "EqualityConstraintTask",
+    "KineticEnergyRegularizationTask",
 )

mink/tasks/com_task.py

@@ -9,7 +9,7 @@ import numpy as np
 import numpy.typing as npt
 
 from ..configuration import Configuration
-from .exceptions import InvalidTarget, TargetNotSet, TaskDefinitionError
+from ..exceptions import InvalidTarget, TargetNotSet, TaskDefinitionError
 from .task import Task
 
 
@@ -18,6 +18,20 @@ class ComTask(Task):
 
     Attributes:
         target_com: Target position of the CoM.
+
+    Example:
+
+    .. code-block:: python
+
+        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)
     """
 
     k: int = 3
@@ -69,7 +83,21 @@ class ComTask(Task):
         self.set_target(configuration.data.subtree_com[1])
 
     def compute_error(self, configuration: Configuration) -> np.ndarray:
-        """Compute the CoM task error.
+        r"""Compute the CoM task error.
+
+        The center of mass :math:`c(q)` for a collection of bodies :math:`\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 :math:`e(q)` is the difference between the current CoM
+        :math:`c(q)` and the target CoM :math:`c^*`:
+
+        .. math::
+
+            e(q) = c(q) - c^*
 
         Args:
             configuration: Robot configuration :math:`q`.
@@ -79,19 +107,46 @@ class ComTask(Task):
         """
         if self.target_com is None:
             raise TargetNotSet(self.__class__.__name__)
+        # Note: body 0 is the world body, so we start from body 1 (the robot).
+        # TODO(kevin): Don't hardcode subtree index.
         return configuration.data.subtree_com[1] - self.target_com
 
     def compute_jacobian(self, configuration: Configuration) -> np.ndarray:
-        """Compute the CoM task Jacobian.
+        r"""Compute the Jacobian of the CoM task error :math:`e(q)`.
+
+        The Jacobian is the derivative of this error with respect to the
+        generalized coordinates :math:`q`. Since the target :math:`c^*` is
+        constant, the Jacobian of the error simplifies to the Jacobian of the
+        CoM position :math:`c(q)`:
+
+        .. math::
+
+            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:
+
+        .. math::
+
+            \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 :math:`M = \sum_{i \in \mathcal{B}} m_i` is the total mass of the subtree,
+        :math:`m_i` is the mass of body :math:`i`, :math:`p_i(q)` is the position
+        of the origin of body frame :math:`i` in the world frame, :math:`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 :math:`i`, and the sum is over
+        all bodies :math:`\mathcal{B}` in the specified subtree (body 1 and its
+        descendants).
 
         Args:
             configuration: Robot configuration :math:`q`.
 
         Returns:
-            Center-of-mass task jacobian :math:`J(q)`.
+            Jacobian of the center-of-mass task error :math:`J(q)`.
         """
-        if self.target_com is None:
-            raise TargetNotSet(self.__class__.__name__)
+        # NOTE: We don't need a target CoM to compute this Jacobian.
         jac = np.empty((self.k, configuration.nv))
         mujoco.mj_jacSubtreeCom(configuration.model, configuration.data, jac, 1)
         return jac

mink/tasks/damping_task.py

@@ -1,20 +1,66 @@
 """Damping task implementation."""
 
-from __future__ import annotations
-
 import mujoco
+import numpy as np
 import numpy.typing as npt
 
+from ..configuration import Configuration
 from .posture_task import PostureTask
 
 
 class DampingTask(PostureTask):
-    """Minimize joint velocities.
+    r"""L2-regularization on joint velocities (a.k.a. *velocity damping*).
+
+    This low-priority task adds a Tikhonov/Levenberg-Marquardt term to the
+    quadratic program, making the Hessian strictly positive-definite and
+    selecting the **minimum-norm joint velocity** in any redundant or
+    near-singular situation. Formally it contributes:
+
+    .. math::
+        \frac{1}{2}\,\Delta \mathbf{q}^\top \Lambda\,\Delta \mathbf{q},
+
+    where :math:`\Delta \mathbf{q}\in\mathbb{R}^{n_v}` is the vector of joint
+    displacements and :math:`\Lambda = \mathrm{diag}(\lambda_1, \ldots, \lambda_{n_v})`
+    is a diagonal matrix of per-DoF weights provided by ``cost``. A larger
+    :math:`\lambda_i` reduces motion in DoF :math:`i`; with no other active
+    tasks the robot remains at rest.
+
+    This task does not favor a particular posture—only small instantaneous
+    motion. If you need a posture bias, use an explicit :class:`~.PostureTask`.
+
+    .. note::
+
+        Floating-base coordinates are not affected by this task.
+
+    Example:
 
-    This task is implemented as a special case of the PostureTask where the gain and
-    target configuration are set to 0 and qpos0 respectively.
+    .. code-block:: python
+
+        # 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)
     """
 
     def __init__(self, model: mujoco.MjModel, cost: npt.ArrayLike):
-        super().__init__(model=model, cost=cost, gain=0.0, lm_damping=0.0)
-        self.target_q = model.qpos0
+        super().__init__(model, cost, gain=0.0, lm_damping=0.0)
+
+    def compute_error(self, configuration: Configuration) -> np.ndarray:
+        r"""Compute the damping task error.
+
+        The damping task does not chase a reference; its desired joint velocity
+        is identically **zero**, so the error vector is always
+
+        .. math:: e = \mathbf 0 \in \mathbb R^{n_v}.
+
+        Args:
+            configuration: Robot configuration :math:`q`.
+
+        Returns:
+            Zero vector of length :math:`n_v`.
+        """
+        return np.zeros(configuration.nv)

mink/tasks/equality_constraint_task.py

@@ -10,7 +10,7 @@ import numpy as np
 import numpy.typing as npt
 
 from ..configuration import Configuration
-from .exceptions import InvalidConstraint, TaskDefinitionError
+from ..exceptions import InvalidConstraint, TaskDefinitionError
 from .task import Task
 
 
@@ -24,20 +24,54 @@ def _get_constraint_dim(constraint: int) -> int:
     }[constraint]
 
 
+def _sparse2dense_fallback(
+    res: np.ndarray,
+    mat: np.ndarray,
+    rownnz: np.ndarray,
+    rowadr: np.ndarray,
+    colind: np.ndarray,
+) -> None:
+    """Fallback implementation of mujoco.mju_sparse2dense for Python 3.8.
+
+    This is a fallback implementation of mujoco.mju_sparse2dense for Python 3.8.
+    It is used when the version of MuJoCo is less than 3.2.5. This is because
+    mujoco.mju_sparse2dense was added Oct 23, 2024 which corresponds to 3.2.5+ and
+    Python 3.8 was dropped in 3.2.4.
+    """
+    # Ref: https://github.com/google-deepmind/mujoco/blob/178fb49c2b2ff48f59653515ab09b9cafca31b7a/src/engine/engine_util_sparse.c#L135
+    for r in range(res.shape[0]):
+        for i in range(rownnz[r]):
+            res[r, colind[rowadr[r] + i]] = mat[rowadr[r] + i]
+
+
 def _get_dense_constraint_jacobian(
     model: mujoco.MjModel, data: mujoco.MjData
 ) -> np.ndarray:
     """Return the dense constraint Jacobian for a model."""
-    if mujoco.mj_isSparse(model):
-        efc_J = np.empty((data.nefc, model.nv))
-        mujoco.mju_sparse2dense(
-            efc_J,
-            data.efc_J,
-            data.efc_J_rownnz,
-            data.efc_J_rowadr,
-            data.efc_J_colind,
-        )
+
+    def _sparse2dense(data: mujoco.MjData) -> np.ndarray:
+        if mujoco.__version__ < "3.2.5":
+            efc_J = np.zeros((data.nefc, model.nv))  # Important to zero out here.
+            _sparse2dense_fallback(
+                efc_J,
+                data.efc_J,
+                data.efc_J_rownnz,
+                data.efc_J_rowadr,
+                data.efc_J_colind,
+            )
+        else:
+            efc_J = np.empty((data.nefc, model.nv))
+            mujoco.mju_sparse2dense(
+                efc_J,
+                data.efc_J,
+                data.efc_J_rownnz,
+                data.efc_J_rowadr,
+                data.efc_J_colind,
+            )
         return efc_J
+
+    if mujoco.mj_isSparse(model):
+        return _sparse2dense(data)
     return data.efc_J.reshape((data.nefc, model.nv)).copy()
 
 
@@ -53,9 +87,18 @@ class EqualityConstraintTask(Task):
     * ``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 a model or a specific subset
+    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 :func:`compute_error` and :func:`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]_.
+
     Attributes:
         equalities: ID or name of the equality constraints to regulate. If not provided,
             the task will regulate all equality constraints in the model.
@@ -126,32 +169,25 @@ class EqualityConstraintTask(Task):
         self.cost = np.repeat(self._cost, repeats)
 
     def compute_error(self, configuration: Configuration) -> np.ndarray:
-        """Compute the equality constraint task error.
+        """Compute the task error (constraint residual) :math:`e(q) = r(q)`.
 
         Args:
             configuration: Robot configuration :math:`q`.
 
         Returns:
-            Equality constraint task error vector :math:`e(q)`. The shape of the
-            error vector is ``(neq_active * constraint_dim,)``, where ``neq_active``
-            is the number of active equality constraints, and ``constraint_dim``
-            depends on the type of equality constraint.
+            Task error vector :math:`e(q)` for the active equality constraints.
         """
         self._update_active_constraints(configuration)
         return configuration.data.efc_pos[self._mask]
 
     def compute_jacobian(self, configuration: Configuration) -> np.ndarray:
-        """Compute the task Jacobian at a given configuration.
+        """Compute the task Jacobian (constraint Jacobian) :math:`J(q) = J_r(q)`.
 
         Args:
             configuration: Robot configuration :math:`q`.
 
         Returns:
-            Equality constraint task jacobian :math:`J(q)`. The shape of the Jacobian
-            is ``(neq_active * constraint_dim, nv)``, where ``neq_active`` is the
-            number of active equality constraints, ``constraint_dim`` depends on the
-            type of equality constraint, and ``nv`` is the dimension of the tangent
-            space.
+            Task jacobian :math:`J(q)` for the active equality constraints.
         """
         self._update_active_constraints(configuration)
         efc_J = _get_dense_constraint_jacobian(configuration.model, configuration.data)

mink/tasks/frame_task.py

@@ -8,19 +8,38 @@ import numpy as np
 import numpy.typing as npt
 
 from ..configuration import Configuration
+from ..exceptions import TargetNotSet, TaskDefinitionError
 from ..lie import SE3
-from .exceptions import TargetNotSet, TaskDefinitionError
 from .task import Task
 
 
 class FrameTask(Task):
-    """Regulate the pose of a frame expressed in the world frame.
+    """Regulate the position and orientation of a frame expressed in the world frame.
 
     Attributes:
         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_frame_to_world: Target pose of the frame.
+
+    Example:
+
+    .. code-block:: python
+
+        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)
     """
 
     k: int = 6

mink/tasks/kinetic_energy_regularization_task.py

@@ -0,0 +1,74 @@
+"""Kinetic energy regularization task implementation."""
+
+from __future__ import annotations
+
+from typing import Optional, SupportsFloat
+
+import numpy as np
+
+from ..configuration import Configuration
+from ..exceptions import IntegrationTimestepNotSet, TaskDefinitionError
+from .task import BaseTask, Objective
+
+
+class KineticEnergyRegularizationTask(BaseTask):
+    r"""Kinetic-energy regularization task.
+
+    This low-priority task adds a configuration-dependent quadratic term to the
+    QP objective that penalizes the system's kinetic energy. Formally, it contributes:
+
+    .. math::
+        \frac{1}{2}\, \lambda\, \Delta \mathbf{q}^\top M(\mathbf{q})\,\Delta \mathbf{q},
+
+    where :math:`\Delta \mathbf{q}\in\mathbb{R}^{n_v}` is the vector of joint
+    displacements, :math:`M(\mathbf{q})` is the joint-space inertia matrix
+    (dependent on the current configuration), and :math:`\lambda` is the scalar
+    strength of the regularization.
+
+    .. note::
+
+        This task penalizes DoFs in proportion to their joint-space inertia, so
+        higher-inertia (i.e., heavier) links will move less. This is in contrast to
+        :class:`~.L2RegularizationTask`, which uniformly damps all DoFs.
+
+    .. warning::
+
+        The timestep :math:`\Delta t` must be set via :meth:`set_dt` before use.
+        This task penalizes velocity via displacement, and omitting `dt` will raise
+        a runtime error.
+
+    Example:
+
+    .. code-block:: python
+
+        task = KineticEnergyRegularizationTask(cost=1e-4)
+        task.set_dt(0.02)
+    """
+
+    def __init__(self, cost: SupportsFloat):
+        cost = float(cost)
+        if cost < 0:
+            raise TaskDefinitionError(f"{self.__class__.__name__} cost should be >= 0")
+        self.cost: float = cost
+
+        # Kinetic energy is defined as T = ½ * q̇ᵀ * M * q̇. Since q̇ ≈ Δq / dt, we
+        # substitute: T ≈ ½ * (Δq / dt)ᵀ * M * (Δq / dt) = ½ * Δqᵀ * (M / dt²) * Δq.
+        # Therefore, we scale the inertia matrix by 1 / dt² to penalize energy in
+        # terms of joint displacements Δq.
+        self.inv_dt_sq: Optional[float] = None
+
+    def set_dt(self, dt: float) -> None:
+        """Set the integration timestep.
+
+        Args:
+            dt: Integration timestep in [s].
+        """
+        self.inv_dt_sq = 1.0 / dt**2
+
+    def compute_qp_objective(self, configuration: Configuration) -> Objective:
+        if self.inv_dt_sq is None:
+            raise IntegrationTimestepNotSet(self.__class__.__name__)
+        M = configuration.get_inertia_matrix()
+        H = self.cost * self.inv_dt_sq * M
+        c = np.zeros(configuration.nv)
+        return Objective(H, c)

mink/tasks/posture_task.py

@@ -9,19 +9,36 @@ import numpy as np
 import numpy.typing as npt
 
 from ..configuration import Configuration
+from ..exceptions import InvalidTarget, TargetNotSet, TaskDefinitionError
 from ..utils import get_freejoint_dims
-from .exceptions import InvalidTarget, TargetNotSet, TaskDefinitionError
 from .task import Task
 
 
 class PostureTask(Task):
     """Regulate the joint angles of the robot towards a desired posture.
 
-    A posture is a vector of actuated joint angles. Floating-base coordinates are not
-    affected by this task.
+    Using this task with a low cost value is useful as a regularizer when the problem
+    is under-constrained.
 
     Attributes:
-        target_q: Target configuration.
+        target_q: Target configuration :math:`q^*`, of shape :math:`(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.
+
+    Example:
+
+    .. code-block:: python
+
+        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)
     """
 
     target_q: Optional[np.ndarray]
@@ -108,8 +125,8 @@ class PostureTask(Task):
             m=configuration.model,
             qvel=qvel,
             dt=1.0,
-            qpos1=configuration.q,
-            qpos2=self.target_q,
+            qpos1=self.target_q,
+            qpos2=configuration.q,
         )
 
         if self._v_ids is not None:
@@ -132,11 +149,7 @@ class PostureTask(Task):
         Returns:
             Posture task jacobian :math:`J(q)`.
         """
-        if self.target_q is None:
-            raise TargetNotSet(self.__class__.__name__)
-
-        jac = -np.eye(configuration.nv)
+        jac = np.eye(configuration.nv)
         if self._v_ids is not None:
             jac[:, self._v_ids] = 0.0
-
         return jac

mink/tasks/relative_frame_task.py

@@ -8,8 +8,8 @@ import numpy as np
 import numpy.typing as npt
 
 from ..configuration import Configuration
+from ..exceptions import TargetNotSet, TaskDefinitionError
 from ..lie import SE3
-from .exceptions import TargetNotSet, TaskDefinitionError
 from .task import Task
 
 

mink/tasks/task.py

@@ -6,23 +6,41 @@ from typing import NamedTuple
 import numpy as np
 
 from ..configuration import Configuration
-from .exceptions import InvalidDamping, InvalidGain
+from ..exceptions import InvalidDamping, InvalidGain
 
 
 class Objective(NamedTuple):
-    r"""Quadratic objective of the form :math:`\frac{1}{2} x^T H x + c^T x`."""
+    r"""Quadratic objective of the form
+    :math:`\frac{1}{2} \Delta q^T H \Delta q + c^T \Delta q`.
+    """
 
     H: np.ndarray
-    """Hessian matrix, of shape (n_v, n_v)"""
+    r"""Hessian matrix, of shape (:math:`n_v`, :math:`n_v`)."""
     c: np.ndarray
-    """Linear vector, of shape (n_v,)."""
+    r"""Linear vector, of shape (:math:`n_v`,)."""
 
     def value(self, x: np.ndarray) -> float:
         """Returns the value of the objective at the input vector."""
         return x.T @ self.H @ x + self.c @ x
 
 
-class Task(abc.ABC):
+class BaseTask(abc.ABC):
+    """Base class for all tasks."""
+
+    @abc.abstractmethod
+    def compute_qp_objective(self, configuration: Configuration) -> Objective:
+        r"""Compute the matrix-vector pair :math:`(H, c)` of the QP objective.
+
+        Args:
+            configuration: Robot configuration :math:`q`.
+
+        Returns:
+            Pair :math:`(H(q), c(q))`.
+        """
+        raise NotImplementedError
+
+
+class Task(BaseTask):
     r"""Abstract base class for kinematic tasks.
 
     Subclasses must implement the configuration-dependent task error

{mink-0.0.9.dist-info → mink-0.0.11.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: mink
-Version: 0.0.9
+Version: 0.0.11
 Summary: mink: MuJoCo inverse kinematics.
 Keywords: inverse,kinematics,mujoco
 Author-email: Kevin Zakka <zakka@berkeley.edu>
@@ -16,6 +16,7 @@ Classifier: Programming Language :: Python :: 3.9
 Classifier: Programming Language :: Python :: 3.10
 Classifier: Programming Language :: Python :: 3.11
 Classifier: Programming Language :: Python :: 3.12
+Classifier: Programming Language :: Python :: 3.13
 Classifier: Topic :: Scientific/Engineering
 License-File: LICENSE
 Requires-Dist: mujoco >= 3.1.6
@@ -27,7 +28,6 @@ Requires-Dist: black ; extra == "dev"
 Requires-Dist: mink[test] ; extra == "dev"
 Requires-Dist: mypy ; extra == "dev"
 Requires-Dist: ruff ; extra == "dev"
-Requires-Dist: dm_control >= 1.0.20 ; extra == "examples"
 Requires-Dist: loop-rate-limiters >= 0.1.0 ; extra == "examples"
 Requires-Dist: qpsolvers[daqp] >= 4.3.1 ; extra == "examples"
 Requires-Dist: osqp >=0.6.2,<1 ; extra == "examples"
@@ -66,14 +66,14 @@ For usage and API reference, see the [documentation](https://kevinzakka.github.i
 If you use mink in your research, please cite it as follows:
 
 ```bibtex
-@software{Zakka_Mink_Python_inverse_2024,
+@software{Zakka_Mink_Python_inverse_2025,
   author = {Zakka, Kevin},
-  license = {Apache-2.0},
-  month = jul,
   title = {{Mink: Python inverse kinematics based on MuJoCo}},
+  year = {2025},
+  month = may,
+  version = {0.0.11},
   url = {https://github.com/kevinzakka/mink},
-  version = {0.0.4},
-  year = {2024}
+  license = {Apache-2.0}
 }
 ```
 

{mink-0.0.9.dist-info → mink-0.0.11.dist-info}/RECORD

@@ -1,9 +1,9 @@
-mink/__init__.py,sha256=g-eOZoPMgO3l4PtP7RK4ercpo1lyLnIcOT6Tb78T968,1829
-mink/configuration.py,sha256=wbP2ab-Zn5pH844Czk5w8mgDXP44AosMOdNxqwyCSik,9199
+mink/__init__.py,sha256=q1A7zX73664z9YJXcW7m2xMhtuZSWMf_eBtjD-NUCDI,2241
+mink/configuration.py,sha256=kYKXS7yuLhXHtJgksrDhaWyyYw8mKoB_tYPijedXHiU,9847
 mink/constants.py,sha256=hcWy4zM4hGI7vvJrTmCjJgs2WtOxKJ1IVtYHucWbOAI,813
-mink/exceptions.py,sha256=tmExo-ydn_mu9Mny5sP0Ehr7hfo3d79fM8HvvxZBKYM,2931
+mink/exceptions.py,sha256=Y8xrcEXtijXaUaf1H02aoIGmRQ7W05cxRKaqxcDp10M,4024
 mink/py.typed,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
-mink/solve_ik.py,sha256=VzPYpfxkJ-JxUZIpwN7-WVOw0Oi2YiIvDZcwB5ROeug,3591
+mink/solve_ik.py,sha256=m_p_woj4InctpkA-JLlAHKQp8Ei9RorR9E9elTNFZYI,3962
 mink/utils.py,sha256=Eok-pr8xR4LjmjEDEOtD7pvZPDCk53K9gWGdth8TxOA,5329
 mink/.mypy_cache/.gitignore,sha256=amnaZw0RUw038PDP3HvtMLeOpkNOJPenMgi5guKdMiw,34
 mink/.mypy_cache/CACHEDIR.TAG,sha256=8cE6_FVTWMkDOw8fMKqhd_6IvaQPS4okWYQA1UeHatw,190
@@ -334,24 +334,23 @@ mink/contrib/keyboard_teleop/teleop_mocap.py,sha256=2cuauiBHsQb6IVruRbGtCumdHQaz
 mink/lie/__init__.py,sha256=7tm3ZFnF3o1SDd9MOFO1In13lHMQJHO0FB-ejI6tsgE,202
 mink/lie/base.py,sha256=8TGWWEWRvmoBsJP-9y1YXQHkTC-TFfI8JDvBFKxxP-A,4865
 mink/lie/se3.py,sha256=cEzAzKEfBAQcbHhbZbhW8QZA8WpfRRaeZ0YGdSmhOx8,8634
-mink/lie/so3.py,sha256=0KgvflDlUJHd_Ruy2pDlgyR7ZC1HkJz4HVxWcCLo5EY,8182
+mink/lie/so3.py,sha256=Goipwo7PRxNecYRNBmueWCiO2D51D7qHlzs2gQiXO-Q,8423
 mink/lie/utils.py,sha256=DuEl3pj84daLvMKN84-YBvkXnJfqvc5vpvJ9J-pJ11U,403
-mink/limits/__init__.py,sha256=hX5Dgpri9AE6YtUCkW79AXMBuNAuBhniR9kQ6Rxwv3Y,416
+mink/limits/__init__.py,sha256=-ijdhSGfMRVE1o1pJ-3qTSsSNIIVxhdk2Z511_UzjzU,343
 mink/limits/collision_avoidance_limit.py,sha256=MJXesSjvm_2O6RUEz2e4D8uTVOHyTsH9j4UxkPemcn8,11918
-mink/limits/configuration_limit.py,sha256=WspxM4W3-VFIKVo8FLg68x7T9EMmfRmzefiWmi4qrZY,4376
-mink/limits/exceptions.py,sha256=EnVKgFhUJFM6rNVCtdngb-XMu66PWKMFSDy-XkdKzr8,178
+mink/limits/configuration_limit.py,sha256=y_1mWbEeuprjcuAgJnFOKdWNI-KqxTPA9X0pW9MF9zQ,4377
 mink/limits/limit.py,sha256=65K4clGFCPxa3TScGaLhYe1rVYgmJnkMnsFg6DCGjI4,1529
-mink/limits/velocity_limit.py,sha256=2i1Jsh6Y4_qw6CR5_hNxQr-X1YkfJtfeLqlsuqj2b8k,3744
-mink/tasks/__init__.py,sha256=rTx9Ab8XPSohhSa1RUD7lNggTzVi02LOaPzPZFMFvuI,763
-mink/tasks/com_task.py,sha256=ch6w7pwa5E0GVo2DCAuEF-c8TCN1iMk-ySY9A4B6yoQ,3121
-mink/tasks/damping_task.py,sha256=SwSfUvMHzoNYNsloQRD1qlPLd4kfoDIWZlTDU9LuKM4,556
-mink/tasks/equality_constraint_task.py,sha256=Ou8jBSC1SsZCi_WbzkgdYog4-rXsUF2uMmwGpFSlBZU,8196
-mink/tasks/exceptions.py,sha256=d2P9q_CWaHUXxgSwFjcu-ml87mLuXJYxrVLUwrJp6w8,803
-mink/tasks/frame_task.py,sha256=DeNg-bEJxqnrRI0FaJK4UHyb8CyF4A7uPF9G-CdN0sg,5307
-mink/tasks/posture_task.py,sha256=sVDZyalCh5DP8zmCuX5kYuZxiMxRut_mTZUjv5y3b5M,3998
-mink/tasks/relative_frame_task.py,sha256=9rIiYocI1eEESt0bLZZpQR7K6ggVRZH8iEhdhzkfZa0,5119
-mink/tasks/task.py,sha256=F16YZT1L9ueNOcKoOhbCyEnZw0DOgrmjqADl0jahVQI,4838
-mink-0.0.9.dist-info/licenses/LICENSE,sha256=xx0jnfkXJvxRnG63LTGOxlggYnIysveWIZ6H3PNdCrQ,11357
-mink-0.0.9.dist-info/WHEEL,sha256=G2gURzTEtmeR8nrdXUJfNiB3VYVxigPQ-bEQujpNiNs,82
-mink-0.0.9.dist-info/METADATA,sha256=PbX9fwluNc1-M5fB8cuJnPD33Flma45GzAU5yd6myfg,5606
-mink-0.0.9.dist-info/RECORD,,
+mink/limits/velocity_limit.py,sha256=drb5tyOymUb_erlghfH7pAZEzfirfvsII1IPrUahSS4,3745
+mink/tasks/__init__.py,sha256=W-wUUrjbnUO17YQpP8Bodm-8p5GJfwTCALS58AroH5Y,624
+mink/tasks/com_task.py,sha256=9QMoUSAt4hUtC2_nWvzK1ZYVWki5P0ttMQp07HW4SKo,5494
+mink/tasks/damping_task.py,sha256=CvUIHeKp0b-YwwzsH7Xr-ctQAQKQMJbmkRIUwbaixio,2279
+mink/tasks/equality_constraint_task.py,sha256=ICC_vN2LB6Sv3T3QxCmbVwQNvhmjtDVaB7-a-PYUNS4,9509
+mink/tasks/frame_task.py,sha256=Y4Jq58cdkpMOs322yQaMgYGBo0CKlQ-W0_wkJmCDKi0,5946
+mink/tasks/kinetic_energy_regularization_task.py,sha256=iCYM8-zYZwu2tNC4jWFBWJyXfIyuCwvqXmFK-X8lMFE,2660
+mink/tasks/posture_task.py,sha256=OPJgjF61QPuU7OqlTT_ThJv0YFs8uJlxcUD8mpZTdt0,4546
+mink/tasks/relative_frame_task.py,sha256=Bu9vBBqzhLKZyBPj1Xc0b69dAa1twkzeLknZDVcRmoA,5120
+mink/tasks/task.py,sha256=BXz17rL_KXiGBrS_5ITqMj2dQXzoQMUUN3xXfGGtJRA,5319
+mink-0.0.11.dist-info/licenses/LICENSE,sha256=xx0jnfkXJvxRnG63LTGOxlggYnIysveWIZ6H3PNdCrQ,11357
+mink-0.0.11.dist-info/WHEEL,sha256=G2gURzTEtmeR8nrdXUJfNiB3VYVxigPQ-bEQujpNiNs,82
+mink-0.0.11.dist-info/METADATA,sha256=BW8wC5vCqRsYktgopCYsOQ_QuZv3oJe_iaVuD6NvoTA,5601
+mink-0.0.11.dist-info/RECORD,,

mink/limits/exceptions.py

@@ -1,7 +0,0 @@
-"""Exceptions raised by limits."""
-
-from ..exceptions import MinkError
-
-
-class LimitDefinitionError(MinkError):
-    """Exception raised when a limit definition is ill-formed."""

mink/tasks/exceptions.py

@@ -1,31 +0,0 @@
-"""Exceptions raised by tasks."""
-
-from ..exceptions import MinkError
-
-
-class TaskDefinitionError(MinkError):
-    """Exception raised when a task definition is ill-formed."""
-
-
-class TargetNotSet(MinkError):
-    """Exception raised when attempting to use a task with an unset target."""
-
-    def __init__(self, cls_name: str):
-        message = f"No target set for {cls_name}"
-        super().__init__(message)
-
-
-class InvalidTarget(MinkError):
-    """Exception raised when the target is invalid."""
-
-
-class InvalidGain(MinkError):
-    """Exception raised when the gain is outside the valid range."""
-
-
-class InvalidDamping(MinkError):
-    """Exception raised when the damping is outside the valid range."""
-
-
-class InvalidConstraint(MinkError):
-    """Exception raised when a constraint is invalid."""