Merge pull request #110 from kevinzakka/add-more-tests

Add more tests to configuration limit.

Author: kevinzakka

CHANGELOG.md

@@ -2,6 +2,12 @@
 
 All notable changes to this project will be documented in this file.
 
+## Unreleased
+
+### Added
+
+* Add more tests to `test_configuration_limit.py` and `test_velocity_limit.py`.
+
 ## [0.0.13] - 2025-09-12
 
 ### Bugfix

mink/lie/se3.py

@@ -38,6 +38,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))
 

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
 

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()

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()