mink 0.0.6__tar.gz → 0.0.8__tar.gz

{mink-0.0.6 → mink-0.0.8}/PKG-INFO

@@ -1,16 +1,17 @@
 Metadata-Version: 2.4
 Name: mink
-Version: 0.0.6
+Version: 0.0.8
 Summary: mink: MuJoCo inverse kinematics.
 Keywords: inverse,kinematics,mujoco
 Author-email: Kevin Zakka <zakka@berkeley.edu>
-Requires-Python: >=3.9
+Requires-Python: >=3.8
 Description-Content-Type: text/markdown
 Classifier: Development Status :: 5 - Production/Stable
 Classifier: Framework :: Robot Framework :: Library
 Classifier: Intended Audience :: Developers
 Classifier: Intended Audience :: Science/Research
 Classifier: License :: OSI Approved :: Apache Software License
+Classifier: Programming Language :: Python :: 3.8
 Classifier: Programming Language :: Python :: 3.9
 Classifier: Programming Language :: Python :: 3.10
 Classifier: Programming Language :: Python :: 3.11
@@ -18,7 +19,7 @@ Classifier: Programming Language :: Python :: 3.12
 Classifier: Topic :: Scientific/Engineering
 License-File: LICENSE
 Requires-Dist: mujoco >= 3.1.6
-Requires-Dist: qpsolvers[quadprog] >= 4.3.1
+Requires-Dist: qpsolvers[daqp] >= 4.3.1
 Requires-Dist: typing_extensions
 Requires-Dist: numpy < 2.0.0
 Requires-Dist: mink[examples, dev] ; extra == "all"
@@ -28,7 +29,8 @@ 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[quadprog, osqp] >= 4.3.1 ; extra == "examples"
+Requires-Dist: qpsolvers[daqp] >= 4.3.1 ; extra == "examples"
+Requires-Dist: osqp >=0.6.2,<1 ; extra == "examples"
 Requires-Dist: absl-py ; extra == "test"
 Requires-Dist: coveralls ; extra == "test"
 Requires-Dist: pytest ; extra == "test"

{mink-0.0.6 → mink-0.0.8}/mink/contrib/keyboard_teleop/teleop_mocap.py

@@ -112,10 +112,7 @@ class TeleopMocap:
         """
 
         q = self.data.mocap_quat[self.mocap_idx].copy()
-        rot = np.zeros(shape=(9,), dtype=np.float64)
-        mujoco.mju_quat2Mat(rot, q)
-        rot = rot.reshape((3, 3))
-        unit_vec = rot[:, axis]
+        unit_vec = self.unit_vec_from_quat(q, axis)
         step_size = self.m_step_size if self.manual else self.nm_step_size
         self.data.mocap_pos[self.mocap_idx] += direction * step_size * unit_vec
 
@@ -125,31 +122,33 @@ class TeleopMocap:
         based on the axis and step size.
         """
 
+        q = self.data.mocap_quat[self.mocap_idx].copy()
+        unit_vec = self.unit_vec_from_quat(q, axis)
+
+        # Rotate the quaternion by the specified angle around the axis.
+        quat_rot = np.zeros(shape=(4,), dtype=np.float64)
+        result = np.zeros(shape=(4,), dtype=np.float64)
         step_size = self.m_rotation_step if self.manual else self.nm_rotation_step
-        self.data.mocap_quat[self.mocap_idx] = self.rotate_quaternion(
-            self.data.mocap_quat[self.mocap_idx], axis, direction * step_size
-        )
+        angle = direction * step_size
+        angle_rad = np.deg2rad(angle)
+        unit_vec = unit_vec / np.linalg.norm(unit_vec)
+        mujoco.mju_axisAngle2Quat(quat_rot, unit_vec, angle_rad)
+        mujoco.mju_mulQuat(result, quat_rot, q)
 
-    def rotate_quaternion(self, quat, axis, angle):
+        self.data.mocap_quat[self.mocap_idx] = result
+
+    def unit_vec_from_quat(self, q, axis):
         """
-        Rotate a quaternion by an angle around an axis.
+        Compute the unit vector corresponding to the specified axis
+        from the given quaternion.
         """
-        rot = np.zeros(shape=(4,), dtype=np.float64)
-        result = np.zeros(shape=(4,), dtype=np.float64)
 
-        unit_axis = np.zeros(shape=(3,), dtype=np.float64)
-        if axis == 0:
-            unit_axis[0] = 1.0
-        elif axis == 1:
-            unit_axis[1] = 1.0
-        elif axis == 2:
-            unit_axis[2] = 1.0
+        rot = np.zeros(shape=(9,), dtype=np.float64)
+        mujoco.mju_quat2Mat(rot, q)
+        rot = rot.reshape((3, 3))
+        unit_vec = rot[:, axis]
 
-        angle_rad = np.deg2rad(angle)
-        unit_axis = unit_axis / np.linalg.norm(unit_axis)
-        mujoco.mju_axisAngle2Quat(rot, unit_axis, angle_rad)
-        mujoco.mju_mulQuat(result, rot, quat)
-        return result
+        return unit_vec
 
     def toggle_on(self):
         self.on = not self.on

{mink-0.0.6 → mink-0.0.8}/mink/lie/base.py

@@ -103,6 +103,21 @@ class MatrixLieGroup(abc.ABC):
         """Normalize/projects values and returns."""
         raise NotImplementedError
 
+    def interpolate(self, other: Self, alpha: float = 0.5) -> Self:
+        """Interpolate between two matrix Lie groups.
+
+        Args:
+            other: The other Lie group, which serves as the end point of interpolation.
+            alpha: The fraction of interpolation between [self, other]. This must
+                be within [0.0, 1.0]. 0.0 = self, 1.0 = other.
+
+        Returns:
+            The interpolated matrix Lie group.
+        """
+        if alpha < 0.0 or alpha > 1.0:
+            raise ValueError(f"Expected alpha within [0.0, 1.0] but received {alpha}")
+        return self @ self.exp(alpha * (self.inverse() @ other).log())
+
     # Plus and minus operators.
 
     # Eqn. 25.

{mink-0.0.6 → mink-0.0.8}/mink/lie/se3.py

@@ -32,6 +32,11 @@ class SE3(MatrixLieGroup):
         xyz = np.round(self.wxyz_xyz[4:], 5)
         return f"{self.__class__.__name__}(wxyz={quat}, xyz={xyz})"
 
+    def __eq__(self, other: object) -> bool:
+        if not isinstance(other, SE3):
+            return NotImplemented
+        return np.array_equal(self.wxyz_xyz, other.wxyz_xyz)
+
     def copy(self) -> SE3:
         return SE3(wxyz_xyz=np.array(self.wxyz_xyz))
 
@@ -54,10 +59,7 @@ class SE3(MatrixLieGroup):
     @classmethod
     def from_rotation(cls, rotation: SO3) -> SE3:
         return SE3.from_rotation_and_translation(
-            rotation=rotation,
-            translation=np.zeros(
-                SE3.space_dim,
-            ),
+            rotation=rotation, translation=np.zeros(SE3.space_dim)
         )
 
     @classmethod

{mink-0.0.6 → mink-0.0.8}/mink/lie/so3.py

@@ -45,6 +45,11 @@ class SO3(MatrixLieGroup):
         wxyz = np.round(self.wxyz, 5)
         return f"{self.__class__.__name__}(wxyz={wxyz})"
 
+    def __eq__(self, other: object) -> bool:
+        if not isinstance(other, SO3):
+            return NotImplemented
+        return np.array_equal(self.wxyz, other.wxyz)
+
     def parameters(self) -> np.ndarray:
         return self.wxyz
 
@@ -79,7 +84,7 @@ class SO3(MatrixLieGroup):
     @classmethod
     def from_matrix(cls, matrix: np.ndarray) -> SO3:
         assert matrix.shape == (SO3.matrix_dim, SO3.matrix_dim)
-        wxyz = np.zeros(SO3.parameters_dim, dtype=np.float64)
+        wxyz = np.empty(SO3.parameters_dim, dtype=np.float64)
         mujoco.mju_mat2Quat(wxyz, matrix.ravel())
         return SO3(wxyz=wxyz)
 
@@ -109,7 +114,7 @@ class SO3(MatrixLieGroup):
 
     # Eq. 138.
     def as_matrix(self) -> np.ndarray:
-        mat = np.zeros(9, dtype=np.float64)
+        mat = np.empty(9, dtype=np.float64)
         mujoco.mju_quat2Mat(mat, self.wxyz)
         return mat.reshape(3, 3)
 

{mink-0.0.6 → mink-0.0.8}/mink/limits/collision_avoidance_limit.py

@@ -2,7 +2,7 @@
 
 import itertools
 from dataclasses import dataclass
-from typing import List, Sequence, Union
+from typing import List, Sequence, Tuple, Union
 
 import mujoco
 import numpy as np
@@ -13,7 +13,7 @@ from .limit import Constraint, Limit
 # Type aliases.
 Geom = Union[int, str]
 GeomSequence = Sequence[Geom]
-CollisionPair = tuple[GeomSequence, GeomSequence]
+CollisionPair = Tuple[GeomSequence, GeomSequence]
 CollisionPairs = Sequence[CollisionPair]
 
 
@@ -231,7 +231,7 @@ class CollisionAvoidanceLimit(Limit):
     def _homogenize_geom_id_list(self, geom_list: GeomSequence) -> List[int]:
         """Take a heterogeneous list of geoms (specified via ID or name) and return
         a homogenous list of IDs (int)."""
-        list_of_int: list[int] = []
+        list_of_int: List[int] = []
         for g in geom_list:
             if isinstance(g, int):
                 list_of_int.append(g)

{mink-0.0.6 → mink-0.0.8}/mink/limits/configuration_limit.py

@@ -1,5 +1,7 @@
 """Joint position limit."""
 
+from typing import List
+
 import mujoco
 import numpy as np
 
@@ -38,7 +40,7 @@ class ConfigurationLimit(Limit):
                 f"{self.__class__.__name__} gain must be in the range (0, 1]"
             )
 
-        index_list: list[int] = []  # DoF indices that are limited.
+        index_list: List[int] = []  # DoF indices that are limited.
         lower = np.full(model.nq, -mujoco.mjMAXVAL)
         upper = np.full(model.nq, mujoco.mjMAXVAL)
         for jnt in range(model.njnt):

{mink-0.0.6 → mink-0.0.8}/mink/limits/velocity_limit.py

@@ -1,6 +1,6 @@
 """Joint velocity limit."""
 
-from typing import Mapping
+from typing import List, Mapping, Optional
 
 import mujoco
 import numpy as np
@@ -28,7 +28,7 @@ class VelocityLimit(Limit):
 
     indices: np.ndarray
     limit: np.ndarray
-    projection_matrix: np.ndarray
+    projection_matrix: Optional[np.ndarray]
 
     def __init__(
         self,
@@ -42,8 +42,8 @@ class VelocityLimit(Limit):
             velocities: Dictionary mapping joint name to maximum allowed magnitude in
                 [m]/[s] for slide joints and [rad]/[s] for hinge joints.
         """
-        limit_list: list[float] = []
-        index_list: list[int] = []
+        limit_list: List[float] = []
+        index_list: List[int] = []
         for joint_name, max_vel in velocities.items():
             jid = model.joint(joint_name).id
             jnt_type = model.jnt_type[jid]
@@ -58,7 +58,7 @@ class VelocityLimit(Limit):
                     f"Got: {max_vel.shape}"
                 )
             index_list.extend(range(vadr, vadr + vdim))
-            limit_list.extend(max_vel.tolist())
+            limit_list.extend(max_vel.tolist())  # type: ignore
 
         self.indices = np.array(index_list)
         self.indices.setflags(write=False)

{mink-0.0.6 → mink-0.0.8}/mink/solve_ik.py

@@ -1,6 +1,6 @@
 """Build and solve the inverse kinematics problem."""
 
-from typing import Optional, Sequence
+from typing import List, Optional, Sequence, Tuple
 
 import numpy as np
 import qpsolvers
@@ -24,11 +24,11 @@ def _compute_qp_objective(
 
 def _compute_qp_inequalities(
     configuration: Configuration, limits: Optional[Sequence[Limit]], dt: float
-) -> tuple[Optional[np.ndarray], Optional[np.ndarray]]:
+) -> Tuple[Optional[np.ndarray], Optional[np.ndarray]]:
     if limits is None:
         limits = [ConfigurationLimit(configuration.model)]
-    G_list: list[np.ndarray] = []
-    h_list: list[np.ndarray] = []
+    G_list: List[np.ndarray] = []
+    h_list: List[np.ndarray] = []
     for limit in limits:
         inequality = limit.compute_qp_inequalities(configuration, dt)
         if not inequality.inactive:

{mink-0.0.6 → mink-0.0.8}/mink/tasks/equality_constraint_task.py

@@ -3,7 +3,7 @@
 from __future__ import annotations
 
 import logging
-from typing import Optional, Sequence
+from typing import List, Optional, Sequence
 
 import mujoco
 import numpy as np
@@ -169,7 +169,7 @@ class EqualityConstraintTask(Task):
     def _resolve_equality_ids(
         self, model: mujoco.MjModel, equalities: Optional[Sequence[int | str]]
     ) -> np.ndarray:
-        eq_ids: list[int] = []
+        eq_ids: List[int] = []
 
         if equalities is not None:
             for eq_id_or_name in equalities:
@@ -186,7 +186,7 @@ class EqualityConstraintTask(Task):
                     eq_id = eq_id_or_name
                     if eq_id < 0 or eq_id >= model.neq:
                         raise InvalidConstraint(
-                            f"Equality constraint index {eq_id} out of range."
+                            f"Equality constraint index {eq_id} out of range. "
                             f"Must be in range [0, {model.neq})."
                         )
                 if not model.eq_active0[eq_id]:

{mink-0.0.6 → mink-0.0.8}/mink/utils.py

@@ -1,4 +1,4 @@
-from typing import Optional
+from typing import List, Optional, Tuple
 
 import mujoco
 import numpy as np
@@ -35,7 +35,7 @@ def move_mocap_to_frame(
     mujoco.mju_mat2Quat(data.mocap_quat[mocap_id], xmat)
 
 
-def get_freejoint_dims(model: mujoco.MjModel) -> tuple[list[int], list[int]]:
+def get_freejoint_dims(model: mujoco.MjModel) -> Tuple[List[int], List[int]]:
     """Get all floating joint configuration and tangent indices.
 
     Args:
@@ -45,8 +45,8 @@ def get_freejoint_dims(model: mujoco.MjModel) -> tuple[list[int], list[int]]:
         A (q_ids, v_ids) pair containing all floating joint indices in the
         configuration and tangent spaces respectively.
     """
-    q_ids: list[int] = []
-    v_ids: list[int] = []
+    q_ids: List[int] = []
+    v_ids: List[int] = []
     for j in range(model.njnt):
         if model.jnt_type[j] == mujoco.mjtJoint.mjJNT_FREE:
             qadr = model.jnt_qposadr[j]
@@ -97,7 +97,7 @@ def custom_configuration_vector(
     return q
 
 
-def get_body_body_ids(model: mujoco.MjModel, body_id: int) -> list[int]:
+def get_body_body_ids(model: mujoco.MjModel, body_id: int) -> List[int]:
     """Get immediate children bodies belonging to a given body.
 
     Args:
@@ -105,7 +105,7 @@ def get_body_body_ids(model: mujoco.MjModel, body_id: int) -> list[int]:
         body_id: ID of body.
 
     Returns:
-        A list containing all child body ids.
+        A List containing all child body ids.
     """
     return [
         i
@@ -115,7 +115,7 @@ def get_body_body_ids(model: mujoco.MjModel, body_id: int) -> list[int]:
     ]
 
 
-def get_subtree_body_ids(model: mujoco.MjModel, body_id: int) -> list[int]:
+def get_subtree_body_ids(model: mujoco.MjModel, body_id: int) -> List[int]:
     """Get all bodies belonging to subtree starting at a given body.
 
     Args:
@@ -123,9 +123,9 @@ def get_subtree_body_ids(model: mujoco.MjModel, body_id: int) -> list[int]:
         body_id: ID of body where subtree starts.
 
     Returns:
-        A list containing all subtree body ids.
+        A List containing all subtree body ids.
     """
-    body_ids: list[int] = []
+    body_ids: List[int] = []
     stack = [body_id]
     while stack:
         body_id = stack.pop()
@@ -134,7 +134,7 @@ def get_subtree_body_ids(model: mujoco.MjModel, body_id: int) -> list[int]:
     return body_ids
 
 
-def get_body_geom_ids(model: mujoco.MjModel, body_id: int) -> list[int]:
+def get_body_geom_ids(model: mujoco.MjModel, body_id: int) -> List[int]:
     """Get immediate geoms belonging to a given body.
 
     Here, immediate geoms are those directly attached to the body and not its
@@ -152,7 +152,7 @@ def get_body_geom_ids(model: mujoco.MjModel, body_id: int) -> list[int]:
     return list(range(geom_start, geom_end))
 
 
-def get_subtree_geom_ids(model: mujoco.MjModel, body_id: int) -> list[int]:
+def get_subtree_geom_ids(model: mujoco.MjModel, body_id: int) -> List[int]:
     """Get all geoms belonging to subtree starting at a given body.
 
     Here, a subtree is defined as the kinematic tree starting at the body and including
@@ -165,7 +165,7 @@ def get_subtree_geom_ids(model: mujoco.MjModel, body_id: int) -> list[int]:
     Returns:
         A list containing all subtree geom ids.
     """
-    geom_ids: list[int] = []
+    geom_ids: List[int] = []
     stack = [body_id]
     while stack:
         body_id = stack.pop()

{mink-0.0.6 → mink-0.0.8}/pyproject.toml

@@ -4,12 +4,12 @@ build-backend = "flit_core.buildapi"
 
 [project]
 name = "mink"
-version = "0.0.6"
+version = "0.0.8"
 readme = "README.md"
 authors = [
     {name = "Kevin Zakka", email = "zakka@berkeley.edu"},
 ]
-requires-python = ">=3.9"
+requires-python = ">=3.8"
 dynamic = ["description"]
 classifiers = [
     "Development Status :: 5 - Production/Stable",
@@ -17,6 +17,7 @@ classifiers = [
     "Intended Audience :: Developers",
     "Intended Audience :: Science/Research",
     "License :: OSI Approved :: Apache Software License",
+    "Programming Language :: Python :: 3.8",
     "Programming Language :: Python :: 3.9",
     "Programming Language :: Python :: 3.10",
     "Programming Language :: Python :: 3.11",
@@ -26,7 +27,7 @@ classifiers = [
 dependencies = [
     # 3.1.6 for mj_geomDistance.
     "mujoco >= 3.1.6",
-    "qpsolvers[quadprog] >= 4.3.1",
+    "qpsolvers[daqp] >= 4.3.1",
     "typing_extensions",
     "numpy < 2.0.0",
 ]
@@ -36,7 +37,8 @@ keywords = ["inverse", "kinematics", "mujoco"]
 examples = [
     "dm_control >= 1.0.20",
     "loop-rate-limiters >= 0.1.0",
-    "qpsolvers[quadprog,osqp] >= 4.3.1",
+    "qpsolvers[daqp] >= 4.3.1",
+    "osqp >=0.6.2,<1",
 ]
 test = [
     "absl-py",