diff --git a/lerobot/common/motors/motors_bus.py b/lerobot/common/motors/motors_bus.py index 71016fb5..b70a728c 100644 --- a/lerobot/common/motors/motors_bus.py +++ b/lerobot/common/motors/motors_bus.py @@ -21,6 +21,7 @@ import abc import logging +from contextlib import contextmanager from dataclasses import dataclass from enum import Enum from functools import cached_property @@ -463,6 +464,14 @@ class MotorsBus(abc.ABC): def enable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None: pass + @contextmanager + def torque_disabled(self): + self.disable_torque() + try: + yield + finally: + self.enable_torque() + def set_timeout(self, timeout_ms: int | None = None): timeout_ms = timeout_ms if timeout_ms is not None else self.default_timeout self.port_handler.setPacketTimeoutMillis(timeout_ms) diff --git a/lerobot/common/robots/koch/koch_follower.py b/lerobot/common/robots/koch/koch_follower.py index fc94f0ea..2395118d 100644 --- a/lerobot/common/robots/koch/koch_follower.py +++ b/lerobot/common/robots/koch/koch_follower.py @@ -146,29 +146,28 @@ class KochFollower(Robot): logger.info(f"Calibration saved to {self.calibration_fpath}") def configure(self) -> None: - self.arm.disable_torque() - self.arm.configure_motors() - # Use 'extended position mode' for all motors except gripper, because in joint mode the servos - # can't rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while - # assembling the arm, you could end up with a servo with a position 0 or 4095 at a crucial - # point - for name in self.arm.names: - if name != "gripper": - self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value) + with self.arm.torque_disabled(): + self.arm.configure_motors() + # Use 'extended position mode' for all motors except gripper, because in joint mode the servos + # can't rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling + # the arm, you could end up with a servo with a position 0 or 4095 at a crucial point + for name in self.arm.names: + if name != "gripper": + self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value) - # Use 'position control current based' for gripper to be limited by the limit of the current. - # For the follower gripper, it means it can grasp an object without forcing too much even tho, - # its goal position is a complete grasp (both gripper fingers are ordered to join and reach a touch). - # For the leader gripper, it means we can use it as a physical trigger, since we can force with our finger - # to make it move, and it will move back to its original target position when we release the force. - self.arm.write("Operating_Mode", "gripper", OperatingMode.CURRENT_POSITION.value) + # Use 'position control current based' for gripper to be limited by the limit of the current. For + # the follower gripper, it means it can grasp an object without forcing too much even tho, its + # goal position is a complete grasp (both gripper fingers are ordered to join and reach a touch). + # For the leader gripper, it means we can use it as a physical trigger, since we can force with + # our finger to make it move, and it will move back to its original target position when we + # release the force. + self.arm.write("Operating_Mode", "gripper", OperatingMode.CURRENT_POSITION.value) - # Set better PID values to close the gap between recorded states and actions - # TODO(rcadene): Implement an automatic procedure to set optimal PID values for each motor - self.arm.write("Position_P_Gain", "elbow_flex", 1500) - self.arm.write("Position_I_Gain", "elbow_flex", 0) - self.arm.write("Position_D_Gain", "elbow_flex", 600) - self.arm.enable_torque() + # Set better PID values to close the gap between recorded states and actions + # TODO(rcadene): Implement an automatic procedure to set optimal PID values for each motor + self.arm.write("Position_P_Gain", "elbow_flex", 1500) + self.arm.write("Position_I_Gain", "elbow_flex", 0) + self.arm.write("Position_D_Gain", "elbow_flex", 600) def get_observation(self) -> dict[str, Any]: if not self.is_connected: diff --git a/lerobot/common/robots/so100/so100_follower.py b/lerobot/common/robots/so100/so100_follower.py index 419b4047..50361fc9 100644 --- a/lerobot/common/robots/so100/so100_follower.py +++ b/lerobot/common/robots/so100/so100_follower.py @@ -144,17 +144,15 @@ class SO100Follower(Robot): print("Calibration saved to", self.calibration_fpath) def configure(self) -> None: - self.arm.disable_torque() - self.arm.configure_motors() - for name in self.arm.names: - self.arm.write("Operating_Mode", name, OperatingMode.POSITION.value) - # Set P_Coefficient to lower value to avoid shakiness (Default is 32) - self.arm.write("P_Coefficient", name, 16) - # Set I_Coefficient and D_Coefficient to default value 0 and 32 - self.arm.write("I_Coefficient", name, 0) - self.arm.write("D_Coefficient", name, 32) - - self.arm.enable_torque() + with self.arm.torque_disabled(): + self.arm.configure_motors() + for name in self.arm.names: + self.arm.write("Operating_Mode", name, OperatingMode.POSITION.value) + # Set P_Coefficient to lower value to avoid shakiness (Default is 32) + self.arm.write("P_Coefficient", name, 16) + # Set I_Coefficient and D_Coefficient to default value 0 and 32 + self.arm.write("I_Coefficient", name, 0) + self.arm.write("D_Coefficient", name, 32) def get_observation(self) -> dict[str, Any]: if not self.is_connected: diff --git a/lerobot/common/robots/viperx/viperx.py b/lerobot/common/robots/viperx/viperx.py index 744fbc87..76287b2d 100644 --- a/lerobot/common/robots/viperx/viperx.py +++ b/lerobot/common/robots/viperx/viperx.py @@ -141,32 +141,31 @@ class ViperX(Robot): logger.info(f"Calibration saved to {self.calibration_fpath}") def configure(self) -> None: - self.arm.disable_torque() - self.arm.configure_motors() + with self.arm.torque_disabled(): + self.arm.configure_motors() - # Set secondary/shadow ID for shoulder and elbow. These joints have two motors. - # As a result, if only one of them is required to move to a certain position, - # the other will follow. This is to avoid breaking the motors. - self.arm.write("Secondary_ID", "shoulder_shadow", 2) - self.arm.write("Secondary_ID", "elbow_shadow", 4) + # Set secondary/shadow ID for shoulder and elbow. These joints have two motors. + # As a result, if only one of them is required to move to a certain position, + # the other will follow. This is to avoid breaking the motors. + self.arm.write("Secondary_ID", "shoulder_shadow", 2) + self.arm.write("Secondary_ID", "elbow_shadow", 4) - # Set a velocity limit of 131 as advised by Trossen Robotics - # TODO(aliberts): remove as it's actually useless in position control - self.arm.write("Velocity_Limit", 131) + # Set a velocity limit of 131 as advised by Trossen Robotics + # TODO(aliberts): remove as it's actually useless in position control + self.arm.write("Velocity_Limit", 131) - # Use 'extended position mode' for all motors except gripper, because in joint mode the servos can't - # rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling the arm, - # you could end up with a servo with a position 0 or 4095 at a crucial point. See: - # https://emanual.robotis.com/docs/en/dxl/x/x_series/#operating-mode11 - for name in self.arm.names: - if name != "gripper": - self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value) + # Use 'extended position mode' for all motors except gripper, because in joint mode the servos + # can't rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling + # the arm, you could end up with a servo with a position 0 or 4095 at a crucial point. + # See: https://emanual.robotis.com/docs/en/dxl/x/x_series/#operating-mode11 + for name in self.arm.names: + if name != "gripper": + self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value) - # Use 'position control current based' for follower gripper to be limited by the limit of the current. - # It can grasp an object without forcing too much even tho, it's goal position is a complete grasp - # (both gripper fingers are ordered to join and reach a touch). - self.arm.write("Operating_Mode", "gripper", OperatingMode.CURRENT_POSITION.value) - self.arm.enable_torque() + # Use 'position control current based' for follower gripper to be limited by the limit of the + # current. It can grasp an object without forcing too much even tho, it's goal position is a + # complete grasp (both gripper fingers are ordered to join and reach a touch). + self.arm.write("Operating_Mode", "gripper", OperatingMode.CURRENT_POSITION.value) def get_observation(self) -> dict[str, Any]: """The returned observations do not have a batch dimension."""