Merge branch 'main' into main

mattrobotcontributor · web-flow · commit e344d52cb937 · 2025-10-03T09:54:29.000-04:00
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -2,12 +2,15 @@
 
 All notable changes to this project will be documented in this file.
 
-## [0.0.14] - 2025-09-26
-
-### Changed
+## Unreleased
 
 - Added joint locking via QP equality constraints as an alternative to removing the joints from the Mujoco model.
 
+### Added
+
+* Improve test coverage.
+* Add more tests to `test_configuration_limit.py` and `test_velocity_limit.py`.
+
 ## [0.0.13] - 2025-09-12
 
 ### Bugfix
diff --git a/mink/configuration.py b/mink/configuration.py
@@ -250,10 +250,10 @@ def get_inertia_matrix(self) -> np.ndarray:
         """
         # Run the composite rigid body inertia (CRB) algorithm to populate the joint
         # space inertia matrix data.qM.
-        if mujoco.mj_version() >= 334:
+        if mujoco.mj_version() >= 334:  # pragma: no branch
             mujoco.mj_makeM(self.model, self.data)
         else:
-            mujoco.mj_crb(self.model, self.data)
+            mujoco.mj_crb(self.model, self.data)  # pragma: no cover
         # 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)
diff --git a/mink/lie/se3.py b/mink/lie/se3.py
@@ -28,6 +28,12 @@ class SE3(MatrixLieGroup):
     tangent_dim: int = 6
     space_dim: int = 3
 
+    def __post_init__(self) -> None:
+        if self.wxyz_xyz.shape != (self.parameters_dim,):
+            raise ValueError(
+                f"Expeced wxyz_xyz to be a length 7 vector but got {self.wxyz_xyz.shape[0]}."
+            )
+
     def __repr__(self) -> str:
         quat = np.round(self.wxyz_xyz[:4], 5)
         xyz = np.round(self.wxyz_xyz[4:], 5)
@@ -38,6 +44,9 @@ def __eq__(self, other: object) -> bool:
             return NotImplemented
         return np.array_equal(self.wxyz_xyz, other.wxyz_xyz)
 
+    def __hash__(self) -> int:
+        return hash(self.wxyz_xyz.tobytes())
+
     def copy(self) -> SE3:
         return SE3(wxyz_xyz=np.array(self.wxyz_xyz))
 
diff --git a/mink/lie/so3.py b/mink/lie/so3.py
@@ -49,6 +49,9 @@ def __eq__(self, other: object) -> bool:
             return NotImplemented
         return np.array_equal(self.wxyz, other.wxyz)
 
+    def __hash__(self) -> int:
+        return hash(self.wxyz.tobytes())
+
     def parameters(self) -> np.ndarray:
         return self.wxyz
 
diff --git a/mink/tasks/equality_constraint_task.py b/mink/tasks/equality_constraint_task.py
@@ -50,7 +50,7 @@ def _get_dense_constraint_jacobian(
     """Return the dense constraint Jacobian for a model."""
 
     def _sparse2dense(data: mujoco.MjData) -> np.ndarray:
-        if mujoco.__version__ < "3.2.5":
+        if mujoco.__version__ < "3.2.5":  # pragma: no cover
             efc_J = np.zeros((data.nefc, model.nv))  # Important to zero out here.
             _sparse2dense_fallback(
                 efc_J,
@@ -59,7 +59,7 @@ def _sparse2dense(data: mujoco.MjData) -> np.ndarray:
                 data.efc_J_rowadr,
                 data.efc_J_colind,
             )
-        else:
+        else:  # pragma: no branch
             efc_J = np.empty((data.nefc, model.nv))
             mujoco.mju_sparse2dense(
                 efc_J,
diff --git a/pyproject.toml b/pyproject.toml
@@ -108,7 +108,6 @@ exclude_also = [
     "if theta_sq < get_epsilon(theta_sq\\.dtype):",
     "def ljac",
     "def rjac",
-    "if mujoco.__version__"
 ]
 omit = [
     "*exceptions.py",
diff --git a/tests/test_collision_avoidance_limit.py b/tests/test_collision_avoidance_limit.py
@@ -133,6 +133,69 @@ def test_contact_normal_jac_matches_mujoco(self):
 
             np.testing.assert_allclose(jac, efc_J, atol=1e-7)
 
+    def test_qp_upper_bound_branches_with_active_contact(self):
+        """Exercise both upper-bound branches with an active contact.
+
+        We place two touching spheres (hi_bound_dist=0) and use a large detection
+        distance so the contact is active. With a positive minimum_distance, h equals
+        the bound_relaxation (else branch); with a negative minimum_distance, h exceeds
+        the relaxation (if branch). G remains identical across both runs.
+        """
+        xml = """
+        <mujoco>
+        <worldbody>
+            <body name="left">
+            <joint type="free"/>
+            <geom name="gL" type="sphere" size="0.01" pos="0 0 0"/>
+            </body>
+            <body name="right">
+            <joint type="free"/>
+            <geom name="gR" type="sphere" size="0.01" pos="0.02 0 0"/>
+            </body>
+        </worldbody>
+        </mujoco>
+        """
+        model = mujoco.MjModel.from_xml_string(xml)
+        cfg = Configuration(model)
+        cfg.update(np.zeros(model.nq))
+
+        detect = 0.5
+        dt = 1e-3
+
+        # hi_bound_dist (0) <= minimum_distance_from_collisions (positive).
+        limit_else = CollisionAvoidanceLimit(
+            model=model,
+            geom_pairs=[(["gL"], ["gR"])],
+            collision_detection_distance=detect,
+            minimum_distance_from_collisions=5e-4,  # 0 <= 5e-4 -> else
+            bound_relaxation=1e-4,
+        )
+        G_else, h_else = limit_else.compute_qp_inequalities(cfg, dt)
+
+        # All rows that correspond to the active pair should be exactly the relaxation.
+        self.assertGreater(h_else.size, 0)
+        self.assertTrue(np.any(np.isfinite(h_else)))
+        self.assertTrue(np.all(h_else[np.isfinite(h_else)] >= 1e-4 - 1e-12))
+
+        # hi_bound_dist (0) > minimum_distance_from_collisions (negative).
+        limit_if = CollisionAvoidanceLimit(
+            model=model,
+            geom_pairs=[(["gL"], ["gR"])],
+            collision_detection_distance=detect,
+            minimum_distance_from_collisions=-5e-3,  # 0 > -5e-3 -> if
+            bound_relaxation=1e-4,
+            gain=0.85,
+        )
+        G_if, h_if = limit_if.compute_qp_inequalities(cfg, dt)
+
+        # h should be strictly larger than relaxation now (gain*dist/dt + relaxation)
+        self.assertGreater(h_if.size, 0)
+        self.assertTrue(np.any(h_if[np.isfinite(h_if)] > 1e-4 + 1e-12))
+
+        # G should be identical for the same geometry, only h changes.
+        self.assertEqual(G_else.shape, G_if.shape)
+        np.testing.assert_allclose(G_else, G_if, atol=0, rtol=0)
+
 
 if __name__ == "__main__":
     absltest.main()
diff --git a/tests/test_configuration_limit.py b/tests/test_configuration_limit.py
@@ -4,6 +4,7 @@
 import numpy as np
 from absl.testing import absltest
 from robot_descriptions.loaders.mujoco import load_robot_description
+from scipy.optimize import linprog
 
 from mink import Configuration
 from mink.exceptions import LimitDefinitionError
@@ -155,6 +156,40 @@ def test_configuration_limit_repulsion(self, tol=1e-10):
         self.assertLess(np.max(h), slack_vel * dt + tol)
         self.assertGreater(np.min(h), -slack_vel * dt - tol)
 
+    def test_configuration_limit_dt_invariance(self):
+        """Inequalities are invariant to dt."""
+        limit = ConfigurationLimit(self.model, gain=0.95)
+        G1, h1 = limit.compute_qp_inequalities(self.configuration, dt=1e-3)
+        G2, h2 = limit.compute_qp_inequalities(self.configuration, dt=0.2)
+        self.assertTrue(np.allclose(G1, G2))
+        self.assertTrue(np.allclose(h1, h2))
+
+    def test_feasible_step_respects_position_bounds(self, tol=1e-9):
+        """A strictly feasible Δq (for GΔq ≤ h) keeps q_next inside [lower, upper]."""
+        limit = ConfigurationLimit(self.model, gain=1.0)
+
+        # dt is irrelevant for configuration limits; pass anything.
+        G, h = limit.compute_qp_inequalities(self.configuration, dt=0.1)
+
+        # We use linprog to construct a strictly feasible delta_q for the inequality
+        # set `G Δq ≤ h - eps``. Shrinking the RHS by eps guarantees strict slack if a
+        # solution exists. linprog with a zero cost is a convenient feasibility solver.
+        eps = 1e-3
+        c = np.zeros(self.configuration.nv)
+        res = linprog(c, A_ub=G, b_ub=h - eps, bounds=(None, None), method="highs")
+        assert res.success, "Could not find a strictly feasible Δq"
+        delta_q = res.x
+
+        # Integrate one second with `v = delta_q` (so Δq = v * 1).
+        q_next = self.configuration.integrate(delta_q, dt=1.0)
+
+        # Check the next configuration is inside the true bounds for limited joints.
+        lower = limit.lower[limit.indices]
+        upper = limit.upper[limit.indices]
+        qn = q_next[limit.indices]
+        assert np.all(qn <= upper + tol)
+        assert np.all(qn >= lower - tol)
+
 
 if __name__ == "__main__":
     absltest.main()
diff --git a/tests/test_lie_operations.py b/tests/test_lie_operations.py
@@ -8,7 +8,7 @@
 
 from mink.exceptions import InvalidMocapBody
 from mink.lie.base import MatrixLieGroup
-from mink.lie.se3 import SE3
+from mink.lie.se3 import SE3, _getQ
 from mink.lie.so3 import SO3
 
 from .utils import assert_transforms_close
@@ -142,6 +142,12 @@ def test_so3_apply(self):
         rotated_vec = rot.apply(vec)
         np.testing.assert_allclose(rotated_vec, rot.as_matrix() @ vec)
 
+    def test_so3_matmul_with_vector_calls_apply(self):
+        """Using @ with an ndarray should delegate to .apply(...)."""
+        vec = np.random.rand(3)
+        rot = SO3.sample_uniform()
+        np.testing.assert_allclose(rot @ vec, rot.apply(vec))
+
     def test_so3_apply_throws_assertion_error_if_wrong_shape(self):
         rot = SO3.sample_uniform()
         vec = np.random.rand(2)
@@ -178,6 +184,10 @@ def test_so3_clamp(self):
 
     # SE3.
 
+    def test_se3_raises_error_if_invalid_shape(self):
+        with self.assertRaises(ValueError):
+            SE3(wxyz_xyz=np.random.rand(2))
+
     def test_se3_equality(self):
         pose_1 = SE3.identity()
         pose_2 = SE3.identity()
@@ -201,6 +211,12 @@ def test_se3_apply(self):
             T.apply(v), T.as_matrix()[:3, :3] @ v + T.translation()
         )
 
+    def test_se3_matmul_with_vector_calls_apply(self):
+        """Using @ with an ndarray should delegate to .apply(...)."""
+        vec = np.random.rand(3)
+        T = SE3.sample_uniform()
+        np.testing.assert_allclose(T @ vec, T.apply(vec))
+
     def test_se3_from_mocap_id(self):
         xml_str = """
         <mujoco>
@@ -335,5 +351,57 @@ def test_se3_clamp(self):
         self.assertAlmostEqual(clamped_rpy.yaw, np.pi)
 
 
+class TestHashAndSetMembership(absltest.TestCase):
+    """Test that SO3 and SE3 objects can be hashed and used in sets."""
+
+    def test_so3_hash_and_set_membership(self):
+        a = SO3.from_rpy_radians(0.1, -0.2, 0.3)
+        b = SO3(wxyz=a.wxyz.copy())
+        c = SO3.from_rpy_radians(0.1, -0.2, 0.31)
+        assert a == b
+        assert hash(a) == hash(b)
+        s = {a, b, c}
+        assert a in s and b in s and c in s
+        assert len(s) == 2
+
+    def test_se3_hash_and_set_membership(self):
+        R = SO3.from_rpy_radians(0.05, 0.02, -0.01)
+        t = np.array([0.3, -0.1, 0.2], dtype=np.float64)
+        a = SE3.from_rotation_and_translation(R, t)
+        b = SE3.from_rotation_and_translation(SO3(wxyz=R.wxyz.copy()), t.copy())
+        c = SE3.from_rotation_and_translation(R, t + np.array([1e-3, 0.0, 0.0]))
+        assert a == b
+        assert hash(a) == hash(b)
+        s = {a, b, c}
+        assert len(s) == 2
+
+
+class TestSE3_getQ(absltest.TestCase):
+    """Covers both small-angle and general branches in mink.lie.se3._getQ."""
+
+    def test__getQ_small_angle_zero(self):
+        # theta == 0 (small-angle branch); with v=0 too, Q should be exactly zero.
+        c_small = np.zeros(6, dtype=np.float64)
+        Q = _getQ(c_small)
+        np.testing.assert_allclose(Q, np.zeros((3, 3), dtype=np.float64))
+
+    def test__getQ_general_branch_nontrivial(self):
+        # Non-zero rotation (general branch).
+        # Use non-zero v to avoid the trivial zero result from the small-angle test.
+        c = np.zeros(6, dtype=np.float64)
+        c[:3] = np.array([0.3, -0.2, 0.1], dtype=np.float64)  # v
+        c[3:] = np.array([0.4, 0.0, 0.0], dtype=np.float64)  # omega (theta ≈ 0.4 > 0)
+
+        Q = _getQ(c)
+        self.assertEqual(Q.shape, (3, 3))
+        self.assertTrue(np.isfinite(Q).all())
+
+        # Sanity: changing v (with same omega) should change Q.
+        c_scaled = c.copy()
+        c_scaled[:3] *= 2.0
+        Q_scaled = _getQ(c_scaled)
+        self.assertGreater(np.linalg.norm(Q - Q_scaled), 1e-9)
+
+
 if __name__ == "__main__":
     absltest.main()
diff --git a/tests/test_velocity_limit.py b/tests/test_velocity_limit.py
@@ -140,6 +140,14 @@ def test_that_freejoint_raises_error(self):
         expected_error_message = "Free joint floating is not supported"
         self.assertEqual(str(cm.exception), expected_error_message)
 
+    def test_velocity_limit_scales_with_dt(self):
+        """RHS scales linearly with dt (sanity check for Δq = v*dt)."""
+        vlim = VelocityLimit(self.model, self.velocities)
+        G1, h1 = vlim.compute_qp_inequalities(self.configuration, dt=0.1)
+        G2, h2 = vlim.compute_qp_inequalities(self.configuration, dt=0.2)
+        assert np.allclose(G1, G2)
+        assert np.allclose(h2, 2.0 * h1)
+
 
 if __name__ == "__main__":
     absltest.main()

Original file line number	Diff line number	Diff line change
`@@ -108,7 +108,6 @@ exclude_also = [`
`108`	`108`	`"if theta_sq < get_epsilon(theta_sq\\.dtype):",`
`109`	`109`	`"def ljac",`
`110`	`110`	`"def rjac",`
`111`		`- "if mujoco.__version__"`
`112`	`111`	`]`
`113`	`112`	`omit = [`
`114`	`113`	`"*exceptions.py",`