diff --git a/examples/1_leader_arm_teleop_so101.py b/examples/1_leader_arm_teleop_so101.py index 33d0ddd..321a129 100644 --- a/examples/1_leader_arm_teleop_so101.py +++ b/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) diff --git a/examples/common/sts3215_bus.py b/examples/common/sts3215_bus.py index 1c5d683..8a08041 100644 --- a/examples/common/sts3215_bus.py +++ b/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) diff --git a/so101_controller.py b/so101_controller.py index 2d2125d..cdb4fe5 100644 --- a/so101_controller.py +++ b/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()