Feat: Added motor torque toggle to the enable/disable behaviour for Example1

examples/1_leader_arm_teleop_so101.py

@@ -113,6 +113,15 @@ def main() -> None:
             debug_mode=False,
         )
         robot_controller.start_control_loop()
+        # Start with motor torque disabled for safety; controller will
+        # resume/enable torque when the user explicitly enables robot.
+        try:
+            if robot_controller.disable_motors():
+                print("✓ Motors start disabled")
+            else:
+                print("⚠️ Failed to disable motors on start")
+        except Exception as e:
+            print(f"⚠️ Exception disabling motors on start: {e}")
         print("📊 Starting joint state thread...")
         joint_state_thread_obj = threading.Thread(
             target=joint_state_thread,
@@ -135,19 +144,23 @@ def main() -> None:
     if use_real_robot:
         visualizer.add_homing_controls()
         visualizer.add_toggle_robot_enabled_status_button()
+        # Ensure the toggle reflects motors being disabled at startup
+        visualizer.update_toggle_robot_enabled_status(False)
 
         def toggle_robot_enabled() -> None:
             assert robot_controller is not None
             state = data_manager.get_robot_activity_state()
             if state == RobotActivityState.ENABLED:
-                data_manager.set_robot_activity_state(RobotActivityState.DISABLED)
-                robot_controller.graceful_stop()
-                data_manager.set_teleop_state(False, None, None)
-                data_manager.set_leader_teleop_engaged(False)
-                visualizer.update_toggle_robot_enabled_status(False)
-                print("✓ 🔴 Robot disabled")
+                if robot_controller.disable_motors():
+                    data_manager.set_robot_activity_state(RobotActivityState.DISABLED)
+                    data_manager.set_teleop_state(False, None, None)
+                    data_manager.set_leader_teleop_engaged(False)
+                    visualizer.update_toggle_robot_enabled_status(False)
+                    print("✓ 🔴 Robot disabled")
+                else:
+                    print("✗ Failed to disable robot")
             elif state == RobotActivityState.DISABLED:
-                if robot_controller.resume_robot():
+                if robot_controller.enable_motors():
                     data_manager.set_robot_activity_state(RobotActivityState.ENABLED)
                     data_manager.set_leader_teleop_engaged(True)
                     visualizer.update_toggle_robot_enabled_status(True)

examples/common/sts3215_bus.py

@@ -335,6 +335,17 @@ def disconnect(self, disable_torque: bool = True) -> None:
                 self._write_1byte(info["id"], ADDR_LOCK, 0)
         self._port_handler.closePort()
 
+    def set_all_torque(self, enabled: bool) -> None:
+        """Enable or disable torque on all follower motors."""
+        if not self._port_handler.is_open:
+            raise ConnectionError("Serial port is not open")
+        torque_value = 1 if enabled else 0
+        for info in self._calibration.values():
+            motor_id = info["id"]
+            self._write_1byte(motor_id, ADDR_LOCK, 0)
+            self._write_1byte(motor_id, ADDR_TORQUE_ENABLE, torque_value)
+            self._write_1byte(motor_id, ADDR_LOCK, 1)
+
     def _write_1byte(self, motor_id: int, addr: int, value: int) -> None:
         self._packet_handler.write1ByteTxRx(self._port_handler, motor_id, addr, value)
 

so101_controller.py

@@ -269,6 +269,34 @@ def resume_robot(self) -> bool:
             print(f"✗ Resume error: {e}")
             return False
 
+    def enable_motors(self) -> bool:
+        """Enable torque on follower motors and resume the control loop."""
+        if self._robot is None or not self._robot.is_connected:
+            print("✗ Cannot enable motors: follower arm is not connected")
+            return False
+        try:
+            with self._bus_lock:
+                self._robot.set_all_torque(True)
+            return self.resume_robot()
+        except Exception as e:
+            print(f"✗ Enable motors error: {e}")
+            return False
+
+    def disable_motors(self) -> bool:
+        """Disable torque on follower motors and stop sending commands."""
+        if self._robot is None or not self._robot.is_connected:
+            print("✗ Cannot disable motors: follower arm is not connected")
+            return False
+        try:
+            self.graceful_stop()
+            with self._bus_lock:
+                self._robot.set_all_torque(False)
+            print("✓ Motor torque disabled")
+            return True
+        except Exception as e:
+            print(f"✗ Disable motors error: {e}")
+            return False
+
     def is_robot_homed(self, tolerance_degrees: float = 2.0) -> bool:
         """True if current joint angles are near home."""
         cur = self.get_current_joint_angles()