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

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/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/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/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/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/limits/collision_avoidance_limit.py

@@ -206,7 +206,7 @@ class CollisionAvoidanceLimit(Limit):
             jac = compute_contact_normal_jacobian(
                 self.model, configuration.data, contact
             )
-            coefficient_matrix[idx] = -jac
+            coefficient_matrix[idx] = -np.sign(hi_bound_dist) * jac
         return Constraint(G=coefficient_matrix, h=upper_bound)
 
     # Private methods.

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/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/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/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/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/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.dist-info → mink-0.0.13.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: mink
-Version: 0.0.11
+Version: 0.0.13
 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.dist-info → mink-0.0.13.dist-info}/RECORD

@@ -1,9 +1,9 @@
-mink/__init__.py,sha256=q1A7zX73664z9YJXcW7m2xMhtuZSWMf_eBtjD-NUCDI,2241
-mink/configuration.py,sha256=kYKXS7yuLhXHtJgksrDhaWyyYw8mKoB_tYPijedXHiU,9847
+mink/__init__.py,sha256=511PwU7HxnllhOlVaHDF5FtSvx7NzohzYw-KkUWOh9M,2285
+mink/configuration.py,sha256=QGh99QRVJ4sUEj0rSjz6oJBk05_VAHACWCop2wPe_fI,10136
 mink/constants.py,sha256=hcWy4zM4hGI7vvJrTmCjJgs2WtOxKJ1IVtYHucWbOAI,813
-mink/exceptions.py,sha256=Y8xrcEXtijXaUaf1H02aoIGmRQ7W05cxRKaqxcDp10M,4024
+mink/exceptions.py,sha256=-O4mlcOqLgwTfMG-qmWl35hCPnhiPAufr3ioxMD1yWA,4255
 mink/py.typed,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
-mink/solve_ik.py,sha256=m_p_woj4InctpkA-JLlAHKQp8Ei9RorR9E9elTNFZYI,3962
+mink/solve_ik.py,sha256=ZP_z4kiPaa_77PO7hQfRlO6nX5Y4B4qrfzUfR8I1ruU,4622
 mink/utils.py,sha256=Eok-pr8xR4LjmjEDEOtD7pvZPDCk53K9gWGdth8TxOA,5329
 mink/.mypy_cache/.gitignore,sha256=amnaZw0RUw038PDP3HvtMLeOpkNOJPenMgi5guKdMiw,34
 mink/.mypy_cache/CACHEDIR.TAG,sha256=8cE6_FVTWMkDOw8fMKqhd_6IvaQPS4okWYQA1UeHatw,190
@@ -333,24 +333,24 @@ mink/contrib/keyboard_teleop/keycodes.py,sha256=1AN3bWpkw99ZjrkIQnu8QdHSz-_boWMY
 mink/contrib/keyboard_teleop/teleop_mocap.py,sha256=2cuauiBHsQb6IVruRbGtCumdHQazEGvt9w2L50eKGzw,8095
 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=Goipwo7PRxNecYRNBmueWCiO2D51D7qHlzs2gQiXO-Q,8423
+mink/lie/se3.py,sha256=0nrhSE0zgacRBDGDryGTRNLVd9mff-WU4qmBlK6cbdE,10069
+mink/lie/so3.py,sha256=3zmuO31DRXspmBBbD8bOlxYXrb3SZWPg72L_Es1aEYs,9246
 mink/lie/utils.py,sha256=DuEl3pj84daLvMKN84-YBvkXnJfqvc5vpvJ9J-pJ11U,403
 mink/limits/__init__.py,sha256=-ijdhSGfMRVE1o1pJ-3qTSsSNIIVxhdk2Z511_UzjzU,343
-mink/limits/collision_avoidance_limit.py,sha256=MJXesSjvm_2O6RUEz2e4D8uTVOHyTsH9j4UxkPemcn8,11918
+mink/limits/collision_avoidance_limit.py,sha256=ETbtG-99rY9eBd_wb3K2PTqi8GMPmjPA-ZG0MY9x5kc,11943
 mink/limits/configuration_limit.py,sha256=y_1mWbEeuprjcuAgJnFOKdWNI-KqxTPA9X0pW9MF9zQ,4377
 mink/limits/limit.py,sha256=65K4clGFCPxa3TScGaLhYe1rVYgmJnkMnsFg6DCGjI4,1529
 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/com_task.py,sha256=JZoRYiT0F1GObIW8V8gEYELUTpOa681HsrFljvp6heU,5768
+mink/tasks/damping_task.py,sha256=evjHJNibPj8D3Uq3GXuTuGLC03fjeVk8oYBmZm5jtRM,2407
 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/frame_task.py,sha256=2Zd7Sts5C_eGGJ64XYQZ4uZNYvS2PFm347uqSb_Y6Lc,5962
+mink/tasks/kinetic_energy_regularization_task.py,sha256=VpAVA4fpDAZNY96Gx7R2Nim-CDj665sXk0FkfINb00g,2544
+mink/tasks/posture_task.py,sha256=14xFUk02ar14gAMUtKr-ZhGDjylcZkCNYUXSFcM7g4A,4643
 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-0.0.13.dist-info/licenses/LICENSE,sha256=xx0jnfkXJvxRnG63LTGOxlggYnIysveWIZ6H3PNdCrQ,11357
+mink-0.0.13.dist-info/WHEEL,sha256=G2gURzTEtmeR8nrdXUJfNiB3VYVxigPQ-bEQujpNiNs,82
+mink-0.0.13.dist-info/METADATA,sha256=Pi6Uu9f5FSoj9L2uoq8Z_yO9pNTqKaY9xg-fwatAYpg,5758
+mink-0.0.13.dist-info/RECORD,,