mink 0.0.11__tar.gz → 0.0.12__tar.gz

{mink-0.0.11 → mink-0.0.12}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: mink
-Version: 0.0.11
+Version: 0.0.12
 Summary: mink: MuJoCo inverse kinematics.
 Keywords: inverse,kinematics,mujoco
 Author-email: Kevin Zakka <zakka@berkeley.edu>
@@ -59,6 +59,7 @@ Features include:
 * Task specification in configuration or operational space;
 * Limits on joint positions and velocities;
 * Collision avoidance between any geom pair;
+* Support for closed-chain kinematics (loop closures) via [equality constraints](https://mujoco.readthedocs.io/en/stable/computation/index.html#coequality);
 * Lie group interface for rigid body transformations.
 
 For usage and API reference, see the [documentation](https://kevinzakka.github.io/mink/).

{mink-0.0.11 → mink-0.0.12}/README.md

@@ -12,6 +12,7 @@ Features include:
 * Task specification in configuration or operational space;
 * Limits on joint positions and velocities;
 * Collision avoidance between any geom pair;
+* Support for closed-chain kinematics (loop closures) via [equality constraints](https://mujoco.readthedocs.io/en/stable/computation/index.html#coequality);
 * Lie group interface for rigid body transformations.
 
 For usage and API reference, see the [documentation](https://kevinzakka.github.io/mink/).

{mink-0.0.11 → mink-0.0.12}/mink/__init__.py

@@ -19,6 +19,7 @@ from .exceptions import (
     InvalidTarget,
     LimitDefinitionError,
     MinkError,
+    NoSolutionFound,
     NotWithinConfigurationLimits,
     TargetNotSet,
     TaskDefinitionError,
@@ -88,6 +89,7 @@ __all__ = (
     "LimitDefinitionError",
     "TaskDefinitionError",
     "IntegrationTimestepNotSet",
+    "NoSolutionFound",
     "SUPPORTED_FRAMES",
     "FRAME_TO_ENUM",
     "FRAME_TO_JAC_FUNC",

{mink-0.0.11 → mink-0.0.12}/mink/configuration.py

@@ -30,6 +30,7 @@ class Configuration:
     * Running forward kinematics to update the state.
     * Checking configuration limits.
     * Computing Jacobians for different frames.
+    * Computing the joint-space inertia matrix.
     * Retrieving frame transforms relative to the world frame.
     * Integrating velocities to update configurations.
     """
@@ -84,6 +85,10 @@ class Configuration:
             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.
+
+        Raises:
+            NotWithinConfigurationLimits: If the current configuration is outside
+                the joint limits.
         """
         for jnt in range(self.model.njnt):
             jnt_type = self.model.jnt_type[jnt]
@@ -245,7 +250,10 @@ class Configuration:
         """
         # Run the composite rigid body inertia (CRB) algorithm to populate the joint
         # space inertia matrix data.qM.
-        mujoco.mj_crb(self.model, self.data)
+        if mujoco.mj_version() >= 334:
+            mujoco.mj_makeM(self.model, self.data)
+        else:
+            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)

{mink-0.0.11 → mink-0.0.12}/mink/exceptions.py

@@ -9,6 +9,13 @@ class MinkError(Exception):
     """Base class for Mink exceptions."""
 
 
+class NoSolutionFound(MinkError):
+    """Exception raised when the QP solver fails to find a solution."""
+
+    def __init__(self, solver_name: str):
+        super().__init__(f"QP solver {solver_name} failed to find a solution.")
+
+
 class UnsupportedFrame(MinkError):
     """Exception raised when a frame type is unsupported."""
 

{mink-0.0.11 → mink-0.0.12}/mink/lie/se3.py

@@ -1,6 +1,7 @@
 from __future__ import annotations
 
 from dataclasses import dataclass
+from typing import Tuple
 
 import mujoco
 import numpy as np
@@ -221,6 +222,36 @@ class SE3(MatrixLieGroup):
         ljacinv_se3[3:, 3:] = ljacinv_so3
         return ljacinv_se3
 
+    def clamp(
+        self,
+        x_translation: Tuple[float, float] = (-np.inf, np.inf),
+        y_translation: Tuple[float, float] = (-np.inf, np.inf),
+        z_translation: Tuple[float, float] = (-np.inf, np.inf),
+        roll_radians: Tuple[float, float] = (-np.inf, np.inf),
+        pitch_radians: Tuple[float, float] = (-np.inf, np.inf),
+        yaw_radians: Tuple[float, float] = (-np.inf, np.inf),
+    ) -> SE3:
+        """Clamp a SE3 within translation and RPY limits.
+
+        Args:
+            x_translation: The (lower, upper) limits for translation along the x axis.
+            y_translation: The (lower, upper) limits for translation along the y axis.
+            z_translation: The (lower, upper) limits for translation along the z axis.
+            roll_radians: The (lower, upper) limits for the roll.
+            pitch_radians: The (lower, upper) limits for the pitch.
+            yaw_radians: The (lower, upper) limits for the yaw.
+
+        Returns:
+            A SE3 within the translation and RPY limits.
+        """
+        translation_limits = np.array([x_translation, y_translation, z_translation])
+        return SE3.from_rotation_and_translation(
+            rotation=self.rotation().clamp(roll_radians, pitch_radians, yaw_radians),
+            translation=np.clip(
+                self.translation(), translation_limits[:, 0], translation_limits[:, 1]
+            ),
+        )
+
 
 # Eqn 180.
 def _getQ(c) -> np.ndarray:

{mink-0.0.11 → mink-0.0.12}/mink/lie/so3.py

@@ -1,7 +1,7 @@
 from __future__ import annotations
 
 from dataclasses import dataclass
-from typing import NamedTuple
+from typing import NamedTuple, Tuple
 
 import mujoco
 import numpy as np
@@ -260,3 +260,25 @@ class SO3(MatrixLieGroup):
         ljacinv[1, 1] += 1.0
         ljacinv[2, 2] += 1.0
         return ljacinv
+
+    def clamp(
+        self,
+        roll_radians: Tuple[float, float] = (-np.inf, np.inf),
+        pitch_radians: Tuple[float, float] = (-np.inf, np.inf),
+        yaw_radians: Tuple[float, float] = (-np.inf, np.inf),
+    ) -> SO3:
+        """Clamp a SO3 within RPY limits.
+
+        Args:
+            roll_radians: The (lower, upper) limits for the roll.
+            pitch_radians: The (lower, upper) limits for the pitch.
+            yaw_radians: The (lower, upper) limits for the yaw.
+
+        Returns:
+            A SO3 within the RPY limits.
+        """
+        return SO3.from_rpy_radians(
+            roll=np.clip(self.compute_roll_radians(), *roll_radians),
+            pitch=np.clip(self.compute_pitch_radians(), *pitch_radians),
+            yaw=np.clip(self.compute_yaw_radians(), *yaw_radians),
+        )

{mink-0.0.11 → mink-0.0.12}/mink/solve_ik.py

@@ -6,6 +6,7 @@ import numpy as np
 import qpsolvers
 
 from .configuration import Configuration
+from .exceptions import NoSolutionFound
 from .limits import ConfigurationLimit, Limit
 from .tasks import BaseTask, Objective
 
@@ -47,7 +48,18 @@ def build_ik(
     damping: float = 1e-12,
     limits: Optional[Sequence[Limit]] = None,
 ) -> qpsolvers.Problem:
-    """Build quadratic program from current configuration and tasks.
+    r"""Build the quadratic program given the current configuration and tasks.
+
+    The quadratic program is defined as:
+
+    .. math::
+
+        \begin{align*}
+            \min_{\Delta q} & \frac{1}{2} \Delta q^T H \Delta q + c^T \Delta q \\
+            \text{s.t.} \quad & G \Delta q \leq h
+        \end{align*}
+
+    where :math:`\Delta q = v / dt` is the vector of joint displacements.
 
     Args:
         configuration: Robot configuration.
@@ -77,7 +89,7 @@ def solve_ik(
     limits: Optional[Sequence[Limit]] = None,
     **kwargs,
 ) -> np.ndarray:
-    """Solve the differential inverse kinematics problem.
+    r"""Solve the differential inverse kinematics problem.
 
     Computes a velocity tangent to the current robot configuration. The computed
     velocity satisfies at (weighted) best the set of provided kinematic tasks.
@@ -97,13 +109,20 @@ def solve_ik(
             defaults to a configuration limit.
         kwargs: Keyword arguments to forward to the backend QP solver.
 
+    Raises:
+        NotWithinConfigurationLimits: If the current configuration is outside
+            the joint limits and `safety_break` is True.
+        NoSolutionFound: If the QP solver fails to find a solution.
+
     Returns:
-        Velocity `v` in tangent space.
+        Velocity :math:`v` in tangent space.
     """
     configuration.check_limits(safety_break=safety_break)
     problem = build_ik(configuration, tasks, dt, damping, limits)
     result = qpsolvers.solve_problem(problem, solver=solver, **kwargs)
-    dq = result.x
-    assert dq is not None
-    v: np.ndarray = dq / dt
+    if not result.found:
+        raise NoSolutionFound(solver)
+    delta_q = result.x
+    assert delta_q is not None
+    v: np.ndarray = delta_q / dt
     return v

{mink-0.0.11 → mink-0.0.12}/mink/tasks/com_task.py

@@ -49,6 +49,12 @@ class ComTask(Task):
         self.set_cost(cost)
 
     def set_cost(self, cost: npt.ArrayLike) -> None:
+        """Set the cost of the CoM task.
+
+        Args:
+            cost: A vector of shape (1,) (aka identical cost for all coordinates),
+                or (3,) (aka different costs for each coordinate).
+        """
         cost = np.atleast_1d(cost)
         if cost.ndim != 1 or cost.shape[0] not in (1, self.k):
             raise TaskDefinitionError(
@@ -64,7 +70,8 @@ class ComTask(Task):
         """Set the target CoM position in the world frame.
 
         Args:
-            target_com: Desired center-of-mass position in the world frame.
+            target_com: A vector of shape (3,) representing the desired
+                center-of-mass position in the world frame.
         """
         target_com = np.atleast_1d(target_com)
         if target_com.ndim != 1 or target_com.shape[0] != (self.k):

{mink-0.0.11 → mink-0.0.12}/mink/tasks/damping_task.py

@@ -11,26 +11,26 @@ from .posture_task import PostureTask
 class DampingTask(PostureTask):
     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:
+    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:
 
     .. 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`.
+    is a diagonal matrix of per-DoF damping weights specified via ``cost``. A larger
+    :math:`\lambda_i` reduces motion in DoF :math:`i`. With no other active
+    tasks, the robot remains at rest. Unlike the `damping` parameter in
+    :func:`~.solve_ik`, which is uniformly applied to all DoFs, this task does
+    not affect the floating-base coordinates.
 
     .. note::
 
-        Floating-base coordinates are not affected by this task.
+        This task does not favor a particular posture, only small instantaneous motion.
+        If you need a posture bias, use :class:`~.PostureTask` instead.
 
     Example:
 
@@ -53,7 +53,7 @@ class DampingTask(PostureTask):
         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
+        is identically **zero**, so the task error is always:
 
         .. math:: e = \mathbf 0 \in \mathbb R^{n_v}.
 

{mink-0.0.11 → mink-0.0.12}/mink/tasks/frame_task.py

@@ -14,13 +14,13 @@ from .task import Task
 
 
 class FrameTask(Task):
-    """Regulate the position and orientation of a frame expressed in the world frame.
+    """Regulate the position and orientation of a frame of interest on the robot.
 
     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.
+        transform_target_to_world: Target pose of the frame in the world frame.
 
     Example:
 

{mink-0.0.11 → mink-0.0.12}/mink/tasks/kinetic_energy_regularization_task.py

@@ -12,30 +12,29 @@ from .task import BaseTask, Objective
 
 
 class KineticEnergyRegularizationTask(BaseTask):
-    r"""Kinetic-energy regularization task.
+    r"""Kinetic-energy regularization.
 
-    This low-priority task adds a configuration-dependent quadratic term to the
-    QP objective that penalizes the system's kinetic energy. Formally, it contributes:
+    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:
 
     .. 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.
+    displacements, :math:`M(\mathbf{q})` is the joint-space inertia matrix, 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.
+        This task can be seen as an inertia-weighted version of the
+        :class:`~.DampingTask`. Degrees of freedom with higher inertia will move less
+        for the same cost.
 
     .. 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.
+        The integration timestep :math:`\Delta t` must be set via :meth:`set_dt`
+        before use. This ensures the cost is expressed in units of energy (Joules).
 
     Example:
 

{mink-0.0.11 → mink-0.0.12}/mink/tasks/posture_task.py

@@ -15,10 +15,10 @@ from .task import Task
 
 
 class PostureTask(Task):
-    """Regulate the joint angles of the robot towards a desired posture.
+    """Regulate joint angles towards a target posture.
 
-    Using this task with a low cost value is useful as a regularizer when the problem
-    is under-constrained.
+    Often used with a low priority in the task stack, this task acts like a regularizer,
+    biasing the solution toward a specific joint configuration.
 
     Attributes:
         target_q: Target configuration :math:`q^*`, of shape :math:`(n_q,)`. Units are
@@ -83,7 +83,8 @@ class PostureTask(Task):
         """Set the target posture.
 
         Args:
-            target_q: Desired joint configuration.
+            target_q: A vector of shape (nq,) representing the desired joint
+                configuration.
         """
         target_q = np.atleast_1d(target_q)
         if target_q.ndim != 1 or target_q.shape[0] != (self.nq):
@@ -94,7 +95,7 @@ class PostureTask(Task):
         self.target_q = target_q.copy()
 
     def set_target_from_configuration(self, configuration: Configuration) -> None:
-        """Set the target posture from the current configuration.
+        """Set the target posture by extracting it from the current configuration.
 
         Args:
             configuration: Robot configuration :math:`q`.

{mink-0.0.11 → mink-0.0.12}/pyproject.toml

@@ -4,7 +4,7 @@ build-backend = "flit_core.buildapi"
 
 [project]
 name = "mink"
-version = "0.0.11"
+version = "0.0.12"
 readme = "README.md"
 authors = [
     {name = "Kevin Zakka", email = "zakka@berkeley.edu"},
@@ -68,27 +68,27 @@ name = "mink"
 
 [tool.ruff]
 lint.select = [
-    "E",  # pycodestyle errors.
-    "F",  # Pyflakes rules.
+    "E",    # pycodestyle errors.
+    "F",    # Pyflakes rules.
     "PLC",  # Pylint convention warnings.
     "PLE",  # Pylint errors.
     "PLR",  # Pylint refactor recommendations.
     "PLW",  # Pylint warnings.
-    "I"  # Import sorting.
+    "I",    # Import sorting.
 ]
 lint.ignore = [
-    "E741", # Ambiguous variable name. (l, O, or I)
-    "E501",  # Line too long.
+    "E741",     # Ambiguous variable name. (l, O, or I)
+    "E501",     # Line too long.
     "PLR2004",  # Magic value used in comparison.
     "PLR0915",  # Too many statements.
     "PLR0913",  # Too many arguments.
-    "PLC0414",  # Import alias does not rename variable. (this is used for exporting names)
+    "PLC0414",  # Import alias does not rename variable.
     "PLC1901",  # Use falsey strings.
     "PLR5501",  # Use `elif` instead of `else if`.
     "PLR0911",  # Too many return statements.
     "PLR0912",  # Too many branches.
     "PLW0603",  # Global statement updates are discouraged.
-    "PLW2901"  # For loop variable overwritten.
+    "PLW2901",  # For loop variable overwritten.
 ]
 
 [tool.mypy]