wangzixiang
/
lerobot
mirror of https://github.com/huggingface/lerobot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Add torque_disabled context

This commit is contained in:
Simon Alibert 2025-04-15 11:43:22 +02:00
parent 9afc4b771c
commit 2bb73ac431
4 changed files with 59 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
lerobot/common/motors/motors_bus.py
View File

@ -21,6 +21,7 @@
import abc import abc
import logging import logging
from contextlib import contextmanager
from dataclasses import dataclass from dataclasses import dataclass
from enum import Enum from enum import Enum
from functools import cached_property 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: def enable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
pass pass
@contextmanager
def torque_disabled(self):
self.disable_torque()
try:
yield
finally:
self.enable_torque()
def set_timeout(self, timeout_ms: int | None = None): def set_timeout(self, timeout_ms: int | None = None):
timeout_ms = timeout_ms if timeout_ms is not None else self.default_timeout timeout_ms = timeout_ms if timeout_ms is not None else self.default_timeout
self.port_handler.setPacketTimeoutMillis(timeout_ms) self.port_handler.setPacketTimeoutMillis(timeout_ms)

41
lerobot/common/robots/koch/koch_follower.py
View File

@ -146,29 +146,28 @@ class KochFollower(Robot):
logger.info(f"Calibration saved to {self.calibration_fpath}") logger.info(f"Calibration saved to {self.calibration_fpath}")
def configure(self) -> None: def configure(self) -> None:
self.arm.disable_torque() with self.arm.torque_disabled():
self.arm.configure_motors() self.arm.configure_motors()
# Use 'extended position mode' for all motors except gripper, because in joint mode the servos # 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 # can't rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling
# assembling the arm, you could end up with a servo with a position 0 or 4095 at a crucial # the arm, you could end up with a servo with a position 0 or 4095 at a crucial point
# point for name in self.arm.names:
for name in self.arm.names: if name != "gripper":
if name != "gripper": self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value)
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. # Use 'position control current based' for gripper to be limited by the limit of the current. For
# For the follower gripper, it means it can grasp an object without forcing too much even tho, # the follower gripper, it means it can grasp an object without forcing too much even tho, its
# its goal position is a complete grasp (both gripper fingers are ordered to join and reach a touch). # 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 # For the leader gripper, it means we can use it as a physical trigger, since we can force with
# to make it move, and it will move back to its original target position when we release the force. # our finger to make it move, and it will move back to its original target position when we
self.arm.write("Operating_Mode", "gripper", OperatingMode.CURRENT_POSITION.value) # 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 # 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 # 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_P_Gain", "elbow_flex", 1500)
self.arm.write("Position_I_Gain", "elbow_flex", 0) self.arm.write("Position_I_Gain", "elbow_flex", 0)
self.arm.write("Position_D_Gain", "elbow_flex", 600) self.arm.write("Position_D_Gain", "elbow_flex", 600)
self.arm.enable_torque()
def get_observation(self) -> dict[str, Any]: def get_observation(self) -> dict[str, Any]:
if not self.is_connected: if not self.is_connected:

20
lerobot/common/robots/so100/so100_follower.py
View File

@ -144,17 +144,15 @@ class SO100Follower(Robot):
print("Calibration saved to", self.calibration_fpath) print("Calibration saved to", self.calibration_fpath)
def configure(self) -> None: def configure(self) -> None:
self.arm.disable_torque() with self.arm.torque_disabled():
self.arm.configure_motors() self.arm.configure_motors()
for name in self.arm.names: for name in self.arm.names:
self.arm.write("Operating_Mode", name, OperatingMode.POSITION.value) self.arm.write("Operating_Mode", name, OperatingMode.POSITION.value)
# Set P_Coefficient to lower value to avoid shakiness (Default is 32) # Set P_Coefficient to lower value to avoid shakiness (Default is 32)
self.arm.write("P_Coefficient", name, 16) self.arm.write("P_Coefficient", name, 16)
# Set I_Coefficient and D_Coefficient to default value 0 and 32 # Set I_Coefficient and D_Coefficient to default value 0 and 32
self.arm.write("I_Coefficient", name, 0) self.arm.write("I_Coefficient", name, 0)
self.arm.write("D_Coefficient", name, 32) self.arm.write("D_Coefficient", name, 32)
self.arm.enable_torque()
def get_observation(self) -> dict[str, Any]: def get_observation(self) -> dict[str, Any]:
if not self.is_connected: if not self.is_connected:

43
lerobot/common/robots/viperx/viperx.py
View File

@ -141,32 +141,31 @@ class ViperX(Robot):
logger.info(f"Calibration saved to {self.calibration_fpath}") logger.info(f"Calibration saved to {self.calibration_fpath}")
def configure(self) -> None: def configure(self) -> None:
self.arm.disable_torque() with self.arm.torque_disabled():
self.arm.configure_motors() self.arm.configure_motors()
# Set secondary/shadow ID for shoulder and elbow. These joints have two 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, # 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. # the other will follow. This is to avoid breaking the motors.
self.arm.write("Secondary_ID", "shoulder_shadow", 2) self.arm.write("Secondary_ID", "shoulder_shadow", 2)
self.arm.write("Secondary_ID", "elbow_shadow", 4) self.arm.write("Secondary_ID", "elbow_shadow", 4)
# Set a velocity limit of 131 as advised by Trossen Robotics # Set a velocity limit of 131 as advised by Trossen Robotics
# TODO(aliberts): remove as it's actually useless in position control # TODO(aliberts): remove as it's actually useless in position control
self.arm.write("Velocity_Limit", 131) self.arm.write("Velocity_Limit", 131)
# Use 'extended position mode' for all motors except gripper, because in joint mode the servos can't # Use 'extended position mode' for all motors except gripper, because in joint mode the servos
# rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling the arm, # can't rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling
# you could end up with a servo with a position 0 or 4095 at a crucial point. See: # the arm, you could end up with a servo with a position 0 or 4095 at a crucial point.
# https://emanual.robotis.com/docs/en/dxl/x/x_series/#operating-mode11 # See: https://emanual.robotis.com/docs/en/dxl/x/x_series/#operating-mode11
for name in self.arm.names: for name in self.arm.names:
if name != "gripper": if name != "gripper":
self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value) 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. # Use 'position control current based' for follower gripper to be limited by the limit of the
# It can grasp an object without forcing too much even tho, it's goal position is a complete grasp # current. It can grasp an object without forcing too much even tho, it's goal position is a
# (both gripper fingers are ordered to join and reach a touch). # 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.write("Operating_Mode", "gripper", OperatingMode.CURRENT_POSITION.value)
self.arm.enable_torque()
def get_observation(self) -> dict[str, Any]: def get_observation(self) -> dict[str, Any]:
"""The returned observations do not have a batch dimension.""" """The returned observations do not have a batch dimension."""