feat: add reset ability in mujoco

ik_qp/tests/test_stage3_control.py

@@ -159,6 +159,40 @@ def test_active_arm_timeout_faults_both_and_requires_release(profiles):
         controller.close()
 
 
+def test_reset_restores_initial_state_and_requires_fresh_dual_release(profiles):
+    robot = MujocoRobot(profiles)
+    controller = DualArmQpTeleopController(robot, profiles)
+    try:
+        initial = robot.read_joint_positions().positions_rad
+        for arm in ("left", "right"):
+            controller.update_sample(sample(arm, True), 0.0)
+        assert controller.step(0.0).state is SafetyState.ACTIVE
+
+        for arm in ("left", "right"):
+            controller.update_sample(sample(arm, True, [0, 0.005, 0]), 0.01)
+        assert controller.step(0.01).state is SafetyState.ACTIVE
+        moved = robot.read_joint_positions().positions_rad
+        assert any(
+            np.max(np.abs(moved[arm] - initial[arm])) > 1e-5
+            for arm in ("left", "right")
+        )
+
+        reset = controller.reset_to_initial()
+        assert reset.state is SafetyState.RESETTING
+        restored = robot.read_joint_positions().positions_rad
+        for arm in ("left", "right"):
+            np.testing.assert_array_equal(restored[arm], initial[arm])
+
+        assert controller.step(0.02).state is SafetyState.RESETTING
+        controller.update_sample(sample("left", False), 0.03)
+        controller.update_sample(sample("right", True), 0.03)
+        assert controller.step(0.03).state is SafetyState.RESETTING
+        controller.update_sample(sample("right", False), 0.04)
+        assert controller.step(0.04).state is SafetyState.IDLE
+    finally:
+        controller.close()
+
+
 def test_message_compatible_adapter_rejects_wrong_hand():
     message = SimpleNamespace(
         hand="right",