feat: add reset ability in mujoco

ik_qp/src/rm75_ik/teleop_control.py

@@ -17,6 +17,7 @@ from .types import IkOptions, teleop_joint_limits
 class SafetyState(str, Enum):
     IDLE = "idle"
     ACTIVE = "active"
+    RESETTING = "resetting"
     FAULT = "fault"
 
 
@@ -297,6 +298,7 @@ class DualArmQpTeleopController:
         }
         self._latest: Dict[ArmName, tuple[ControllerSample, float]] = {}
         self._fault_reason = ""
+        self._reset_waiting_for_release = False
         self._closed = False
         self.robot.connect()
 
@@ -304,6 +306,8 @@ class DualArmQpTeleopController:
     def safety_state(self) -> SafetyState:
         if self._fault_reason:
             return SafetyState.FAULT
+        if self._reset_waiting_for_release:
+            return SafetyState.RESETTING
         if any(mapper.active for mapper in self.mappers.values()):
             return SafetyState.ACTIVE
         return SafetyState.IDLE
@@ -326,6 +330,27 @@ class DualArmQpTeleopController:
     def reject_input(self, reason: str) -> None:
         self._trip_fault(f"invalid controller input: {reason}")
 
+    def reset_to_initial(self) -> ControlCycleResult:
+        """Reset the backend and require fresh dual-grip release before rearming."""
+
+        if self._closed:
+            raise RuntimeError("controller is closed")
+        for mapper in self.mappers.values():
+            mapper.reset()
+        try:
+            self.robot.stop(("left", "right"))
+            self.robot.reset_to_initial()
+        except Exception as exc:
+            self._trip_fault(f"reset/backend failure: {exc}")
+            raise RuntimeError(f"failed to reset robot backend: {exc}") from exc
+        self._latest.clear()
+        self._fault_reason = ""
+        self._reset_waiting_for_release = True
+        return ControlCycleResult(
+            SafetyState.RESETTING,
+            reason="reset complete; waiting for fresh dual-grip release",
+        )
+
     def step(self, timestamp_sec: float) -> ControlCycleResult:
         if self._closed:
             raise RuntimeError("controller is closed")
@@ -336,6 +361,15 @@ class DualArmQpTeleopController:
                 self._fault_reason = ""
                 return ControlCycleResult(SafetyState.IDLE, reason="fault cleared")
             return ControlCycleResult(SafetyState.FAULT, reason=self._fault_reason)
+        if self._reset_waiting_for_release:
+            if self._can_finish_reset(timestamp_sec):
+                self._reset_waiting_for_release = False
+                self._latest.clear()
+                return ControlCycleResult(SafetyState.IDLE, reason="reset rearmed")
+            return ControlCycleResult(
+                SafetyState.RESETTING,
+                reason="waiting for fresh dual-grip release",
+            )
 
         try:
             state = self.robot.read_joint_positions()
@@ -404,8 +438,22 @@ class DualArmQpTeleopController:
             mapper.reset()
         return True
 
+    def _can_finish_reset(self, timestamp_sec: float) -> bool:
+        if set(self._latest) != {"left", "right"}:
+            return False
+        for arm, (sample, sample_time) in self._latest.items():
+            if sample.grip:
+                return False
+            age = timestamp_sec - sample_time
+            if age < -1e-6 or age > self.profiles[arm].command_timeout_sec:
+                return False
+        for mapper in self.mappers.values():
+            mapper.reset()
+        return True
+
     def _trip_fault(self, reason: str) -> ControlCycleResult:
         self._fault_reason = str(reason)
+        self._reset_waiting_for_release = False
         for mapper in self.mappers.values():
             mapper.reset()
         try:
@@ -415,6 +463,7 @@ class DualArmQpTeleopController:
         return ControlCycleResult(SafetyState.FAULT, reason=self._fault_reason)
 
     def stop(self) -> None:
+        self._reset_waiting_for_release = False
         for mapper in self.mappers.values():
             mapper.reset()
         self.robot.stop(("left", "right"))