Update task and IK solver to support reduced configuration; add tests…

CHANGELOG.md

@@ -14,6 +14,7 @@ All notable changes to this project will be documented in this file.
 
 - Merged task and limit exceptions to base exceptions file.
 - Improved the documentation for certain tasks.
+- Update the task.py and solve_ik.py to support the reduced-configration.
 
 ## [0.0.10] - 2025-04-22
 

mink/solve_ik.py

@@ -13,8 +13,8 @@
 def _compute_qp_objective(
     configuration: Configuration, tasks: Sequence[Task], damping: float
 ) -> Objective:
-    H = np.eye(configuration.model.nv) * damping
-    c = np.zeros(configuration.model.nv)
+    H = np.eye(configuration.nv) * damping
+    c = np.zeros(configuration.nv)
     for task in tasks:
         H_task, c_task = task.compute_qp_objective(configuration)
         H += H_task

mink/tasks/task.py

@@ -130,7 +130,7 @@ def compute_qp_objective(self, configuration: Configuration) -> Objective:
         weighted_error = weight @ minus_gain_error
 
         mu = self.lm_damping * weighted_error @ weighted_error
-        eye_tg = np.eye(configuration.model.nv)
+        eye_tg = np.eye(configuration.nv)
 
         H = weighted_jacobian.T @ weighted_jacobian + mu * eye_tg  # (nv, nv)
         c = -weighted_error.T @ weighted_jacobian  # (nv,)

tests/test_configuration.py

@@ -3,6 +3,8 @@
 import numpy as np
 from absl.testing import absltest
 from robot_descriptions.loaders.mujoco import load_robot_description
+import mujoco
+from typing import Optional
 
 import mink
 
@@ -117,6 +119,161 @@ def test_check_limits_freejoint(self):
         configuration.update(q=q)
         configuration.check_limits(safety_break=True)  # Should not raise.
 
+    def test_reduced_configuration_with_tasks_and_ik(self):
+        """Test that ReducedConfiguration works correctly with tasks and IK solver."""
+        # Create our own ReducedConfiguration class based on the provided implementation
+        class ReducedConfiguration(mink.Configuration):
+            def __init__(self, model, data, relevant_qpos_indices, relevant_qvel_indices):
+                super().__init__(model=model, q=None)
+                self.relevant_qpos_indices = relevant_qpos_indices
+                self.relevant_qvel_indices = relevant_qvel_indices
+
+            @property
+            def q(self) -> np.ndarray:
+                """Return the relevant qpos entries."""
+                return self.data.qpos[self.relevant_qpos_indices].copy()
+
+            @q.setter
+            def q(self, value: np.ndarray):
+                self.data.qpos[self.relevant_qpos_indices] = value
+
+            @property
+            def dq(self) -> np.ndarray:
+                """Return the relevant qvel entries."""
+                return self.data.qvel[self.relevant_qvel_indices].copy()
+
+            @dq.setter
+            def dq(self, value: np.ndarray):
+                self.data.qvel[self.relevant_qvel_indices] = value
+
+            def update(self, q: Optional[np.ndarray] = None) -> None:
+                if q is not None:
+                    self.q = q  
+                super().update()
+
+            def get_frame_jacobian(self, frame_name: str, frame_type: str) -> np.ndarray:
+                full_jacobian = super().get_frame_jacobian(frame_name, frame_type)
+                reduced_jacobian = full_jacobian[:, self.relevant_qvel_indices]
+                return reduced_jacobian
+
+            def integrate_inplace(self, velocity: np.ndarray, dt: float) -> None:
+                full_velocity = np.zeros(self.model.nv)
+                full_velocity[self.relevant_qvel_indices] = velocity
+                super().integrate_inplace(full_velocity, dt)
+
+            @property
+            def nv(self) -> int:
+                return len(self.relevant_qvel_indices)
+
+            def check_limits(self, tol: float = 1e-6, safety_break: bool = True) -> None:
+                """Check that the current configuration is within bounds for relevant joints."""
+                for idx, jnt in enumerate(self.relevant_joints):
+                    jnt_type = self.model.jnt_type[jnt]
+                    if jnt_type == mujoco.mjtJoint.mjJNT_FREE or not self.model.jnt_limited[jnt]:
+                        continue
+                    qval = self.q[idx]
+                    qmin = self.model.jnt_range[jnt, 0]
+                    qmax = self.model.jnt_range[jnt, 1]
+                    if qval < qmin - tol or qval > qmax + tol:
+                        if safety_break:
+                            raise mink.NotWithinConfigurationLimits(
+                                joint_id=jnt,
+                                value=qval,
+                                lower=qmin,
+                                upper=qmax,
+                                model=self.model,
+                            )
+
+            @property
+            def relevant_joints(self) -> np.ndarray:
+                """Get the joint IDs corresponding to the relevant qpos indices."""
+                joint_ids = []
+                for qpos_idx in self.relevant_qpos_indices:
+                    jnt = np.where(self.model.jnt_qposadr == qpos_idx)[0][0]
+                    joint_ids.append(jnt)
+                return np.array(joint_ids)
+
+
+        data = mujoco.MjData(self.model)
+
+        # Create indices for reduced configuration
+        relevant_joints = ["shoulder_pan_joint", "elbow_joint"]
+        relevant_qpos_indices = np.array([
+            self.model.jnt_qposadr[self.model.joint(j).id] 
+            for j in relevant_joints
+        ])
+        relevant_qvel_indices = np.array([
+            self.model.jnt_dofadr[self.model.joint(j).id] 
+            for j in relevant_joints
+        ])
+
+        # Create the reduced configuration
+        configuration = ReducedConfiguration(
+            self.model, 
+            data, 
+            relevant_qpos_indices, 
+            relevant_qvel_indices
+        )
+
+        # Test 1: Verify basic properties
+        self.assertEqual(configuration.nv, len(relevant_qvel_indices))
+        self.assertEqual(len(configuration.q), len(relevant_qpos_indices))
+        self.assertEqual(len(configuration.dq), len(relevant_qvel_indices))
+
+        # Test 2: Verify Jacobian computation
+        jacobian = configuration.get_frame_jacobian("attachment_site", "site")
+        self.assertEqual(jacobian.shape, (6, configuration.nv))
+
+        # Create a task that will use the reduced configuration
+        task = mink.FrameTask(
+            "attachment_site", 
+            "site", 
+            position_cost=1.0, 
+            orientation_cost=0.0
+        )
+
+        # Set a target to ensure we have some error
+        init_transform = configuration.get_transform_frame_to_world("attachment_site", "site")
+        offset = mink.SE3.from_translation(np.array([0.02, 0.0, 0.0]))
+        task.set_target(init_transform @ offset)
+
+        # Test 3: Verify QP objective computation with reduced configuration
+        objective = task.compute_qp_objective(configuration)
+        self.assertEqual(objective.H.shape, (configuration.nv, configuration.nv))
+        self.assertEqual(objective.c.shape, (configuration.nv,))
+
+        # Test 4: Verify IK solver works with reduced configuration
+        velocity = mink.solve_ik(
+            configuration, 
+            [task], 
+            limits=[], 
+            dt=1e-3, 
+            solver="daqp"
+        )
+
+        # Verify velocity shape matches reduced configuration
+        self.assertEqual(velocity.shape, (configuration.nv,))
+
+        # Test 5: Verify the velocity is not all zeros (since target != current)
+        self.assertFalse(np.allclose(velocity, 0.0))
+
+        # Test 6: Verify integration works with reduced configuration
+        dt = 1e-3
+        configuration.integrate_inplace(velocity, dt)
+        self.assertEqual(configuration.q.shape, (len(relevant_qpos_indices),))
+
+        # Test 7: Verify the task error changes after moving
+        error_after = task.compute_error(configuration)
+        self.assertFalse(np.allclose(error_after, 0.0))
+
+        # Test 8: Verify limits checking
+        configuration.check_limits()  # Should not raise
+
+        # Test 9: Verify update method
+        new_q = np.zeros(len(relevant_qpos_indices))
+        configuration.update(q=new_q)
+        np.testing.assert_array_equal(configuration.q, new_q)
+
 
 if __name__ == "__main__":
     absltest.main()

tests/test_solve_ik.py

@@ -6,7 +6,7 @@
 from robot_descriptions.loaders.mujoco import load_robot_description
 
 import mink
-
+import mujoco
 
 class TestSolveIK(absltest.TestCase):
     """Tests for the `solve_ik` function."""
@@ -149,6 +149,105 @@ def test_single_task_convergence(self):
         )
         self.assertLess(nb_steps, 20)
 
+    def test_solve_ik_with_reduced_configuration(self):
+        """Test that solve_ik works with ReducedConfiguration having fewer DoFs."""
+
+        # Use just 2 joints for a reduced configuration
+        relevant_joints = ["shoulder_pan_joint", "elbow_joint"]
+        qpos_indices = np.array([self.model.jnt_qposadr[self.model.joint(j).id] for j in relevant_joints])
+        qvel_indices = np.array([self.model.jnt_dofadr[self.model.joint(j).id] for j in relevant_joints])
+
+        class ReducedConfiguration(mink.Configuration):
+            def __init__(self, model):
+                super().__init__(model)
+                self._qpos_indices = qpos_indices
+                self._qvel_indices = qvel_indices
+
+            @property
+            def q(self):
+                return self.data.qpos[self._qpos_indices].copy()
+
+            @q.setter
+            def q(self, value):
+                self.data.qpos[self._qpos_indices] = value
+
+            @property
+            def dq(self):
+                return self.data.qvel[self._qvel_indices].copy()
+
+            @dq.setter
+            def dq(self, value):
+                self.data.qvel[self._qvel_indices] = value
+
+            def get_frame_jacobian(self, frame_name, frame_type):
+                full_jacobian = super().get_frame_jacobian(frame_name, frame_type)
+                return full_jacobian[:, self._qvel_indices]
+
+            def integrate_inplace(self, velocity, dt):
+                full_velocity = np.zeros(self.model.nv)
+                full_velocity[self._qvel_indices] = velocity
+                super().integrate_inplace(full_velocity, dt)
+
+
+            def check_limits(self, tol: float = 1e-6, safety_break: bool = True) -> None:
+                """Check that the current configuration is within bounds for relevant joints."""
+                for idx, jnt in enumerate(self.relevant_joints):
+                    jnt_type = self.model.jnt_type[jnt]
+                    if jnt_type == mujoco.mjtJoint.mjJNT_FREE or not self.model.jnt_limited[jnt]:
+                        continue
+                    qval = self.q[idx]  # index into reduced q
+                    qmin = self.model.jnt_range[jnt, 0]
+                    qmax = self.model.jnt_range[jnt, 1]
+                    if qval < qmin - tol or qval > qmax + tol:
+                        if safety_break:
+                            raise NotWithinConfigurationLimits(
+                                joint_id=jnt,
+                                value=qval,
+                                lower=qmin,
+                                upper=qmax,
+                                model=self.model,
+                            )
+                        else:
+                            print(
+                                f"Value {qval:.2f} at index {idx} is outside of its limits: "
+                                f"[{qmin:.2f}, {qmax:.2f}]"
+                            )
+
+            @property
+            def nv(self):
+                return len(self._qvel_indices)
+
+
+            @property
+            def relevant_joints(self) -> np.ndarray:
+                """Return joint IDs for the reduced qpos indices."""
+                joint_ids = []
+                for qpos_idx in self._qpos_indices:
+                    jnt = np.where(self.model.jnt_qposadr == qpos_idx)[0]
+                    if len(jnt) == 0:
+                        raise ValueError(f"No joint found for qpos index {qpos_idx}")
+                    joint_ids.append(jnt[0])
+                return np.array(joint_ids)
+
+        configuration = ReducedConfiguration(self.model)
+
+        # Use a task that's reachable by those 2 joints
+        task = mink.FrameTask(
+            "attachment_site", "site", position_cost=1.0, orientation_cost=0.0
+        )
+        init_transform = configuration.get_transform_frame_to_world("attachment_site", "site")
+        offset = mink.SE3.from_translation(np.array([0.02, 0.0, 0.0]))  # small displacement
+        task.set_target(init_transform @ offset)
+
+        velocity = mink.solve_ik(
+            configuration, [task], limits=[], dt=1e-3, solver="daqp"
+        )
+
+        # Check velocity shape matches reduced nv
+        self.assertEqual(velocity.shape, (configuration.nv,))
+        # Check it's not all-zero (since target != current)
+        self.assertFalse(np.allclose(velocity, 0.0))
+
 
 if __name__ == "__main__":
     absltest.main()