From d5ebac067ade4b17f7c7a2bd69ec789ef1d176b9 Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Mon, 26 May 2025 17:39:40 +0200 Subject: [PATCH] Left/Right sides + other fixes --- .../common/robots/hope_jr/config_hope_jr.py | 6 +++ lerobot/common/robots/hope_jr/hope_jr_hand.py | 37 ++++++++++++------ .../robots/hope_jr/joints_translation.py | 14 ++++--- .../homonculus/config_homonculus.py | 5 +++ .../homonculus/homonculus_arm.py | 2 +- .../homonculus/homonculus_glove.py | 39 ++++++++++++++----- 6 files changed, 75 insertions(+), 28 deletions(-) diff --git a/lerobot/common/robots/hope_jr/config_hope_jr.py b/lerobot/common/robots/hope_jr/config_hope_jr.py index 5e4df7aa0..649a2caf8 100644 --- a/lerobot/common/robots/hope_jr/config_hope_jr.py +++ b/lerobot/common/robots/hope_jr/config_hope_jr.py @@ -25,11 +25,17 @@ from ..config import RobotConfig @dataclass class HopeJrHandConfig(RobotConfig): port: str # Port to connect to the hand + side: str # "left" / "right" disable_torque_on_disconnect: bool = True cameras: dict[str, CameraConfig] = field(default_factory=dict) + def __post_init__(self): + super().__post_init__() + if self.side not in ["right", "left"]: + raise ValueError(self.side) + @RobotConfig.register_subclass("hope_jr_arm") @dataclass diff --git a/lerobot/common/robots/hope_jr/hope_jr_hand.py b/lerobot/common/robots/hope_jr/hope_jr_hand.py index 49f6d1996..f32a5b8aa 100644 --- a/lerobot/common/robots/hope_jr/hope_jr_hand.py +++ b/lerobot/common/robots/hope_jr/hope_jr_hand.py @@ -32,6 +32,31 @@ from .config_hope_jr import HopeJrHandConfig logger = logging.getLogger(__name__) +RIGHT_HAND_INVERSIONS = [ + "thumb_mcp", + "thumb_dip", + "index_ulnar_flexor", + "middle_ulnar_flexor", + "ring_ulnar_flexor", + "ring_pip_dip", + "pinky_ulnar_flexor", + "pinky_pip_dip", +] + +LEFT_HAND_INVERSIONS = [ + "thumb_cmc", + "thumb_mcp", + "thumb_dip", + "index_radial_flexor", + "index_pip_dip", + "middle_radial_flexor", + "middle_pip_dip", + "ring_radial_flexor", + "ring_pip_dip", + "pinky_radial_flexor", + "pinky_pip_dip", +] + class HopeJrHand(Robot): config_class = HopeJrHandConfig @@ -69,17 +94,7 @@ class HopeJrHand(Robot): protocol_version=1, ) self.cameras = make_cameras_from_configs(config.cameras) - - self.inverted_motors = [ - "thumb_mcp", - "thumb_dip", - "index_ulnar_flexor", - "middle_ulnar_flexor", - "ring_ulnar_flexor", - "ring_pip_dip", - "pinky_ulnar_flexor", - "pinky_pip_dip", - ] + self.inverted_motors = RIGHT_HAND_INVERSIONS if config.side == "right" else LEFT_HAND_INVERSIONS @property def _motors_ft(self) -> dict[str, type]: diff --git a/lerobot/common/robots/hope_jr/joints_translation.py b/lerobot/common/robots/hope_jr/joints_translation.py index 155d147ef..68c00bffc 100644 --- a/lerobot/common/robots/hope_jr/joints_translation.py +++ b/lerobot/common/robots/hope_jr/joints_translation.py @@ -1,16 +1,18 @@ -INDEX_SPLAY = 0.2 -MIDDLE_SPLAY = 0.1 -RING_SPLAY = 0.1 -PINKY_SPLAY = -0.1 +INDEX_SPLAY = 0.3 +MIDDLE_SPLAY = 0.3 +RING_SPLAY = 0.3 +PINKY_SPLAY = 0.5 def get_ulnar_flexion(flexion: float, abduction: float, splay: float): - ulnar_component = max(-abduction, 0) + # ulnar_component = max(-abduction, 0) + ulnar_component = -abduction return ulnar_component * splay + flexion * (1 - splay) def get_radial_flexion(flexion: float, abduction: float, splay: float): - radial_component = max(abduction, 0) + radial_component = abduction + # radial_component = max(abduction, 0) return radial_component * splay + flexion * (1 - splay) diff --git a/lerobot/common/teleoperators/homonculus/config_homonculus.py b/lerobot/common/teleoperators/homonculus/config_homonculus.py index be5113259..88303d968 100644 --- a/lerobot/common/teleoperators/homonculus/config_homonculus.py +++ b/lerobot/common/teleoperators/homonculus/config_homonculus.py @@ -23,8 +23,13 @@ from ..config import TeleoperatorConfig @dataclass class HomonculusGloveConfig(TeleoperatorConfig): port: str # Port to connect to the glove + side: str # "left" / "right" baud_rate: int = 115_200 + def __post_init__(self): + if self.side not in ["right", "left"]: + raise ValueError(self.side) + @TeleoperatorConfig.register_subclass("homonculus_arm") @dataclass diff --git a/lerobot/common/teleoperators/homonculus/homonculus_arm.py b/lerobot/common/teleoperators/homonculus/homonculus_arm.py index 2768d70ca..0a75787a6 100644 --- a/lerobot/common/teleoperators/homonculus/homonculus_arm.py +++ b/lerobot/common/teleoperators/homonculus/homonculus_arm.py @@ -234,7 +234,7 @@ class HomonculusArm(Teleoperator): "wrist_pitch": int(raw_values[0]), } - with self._lock: + with self.lock: self._state = joint_angles self.new_state_event.set() diff --git a/lerobot/common/teleoperators/homonculus/homonculus_glove.py b/lerobot/common/teleoperators/homonculus/homonculus_glove.py index 81ff7f8f0..aebe6057c 100644 --- a/lerobot/common/teleoperators/homonculus/homonculus_glove.py +++ b/lerobot/common/teleoperators/homonculus/homonculus_glove.py @@ -30,6 +30,28 @@ from .config_homonculus import HomonculusGloveConfig logger = logging.getLogger(__name__) +LEFT_HAND_INVERSIONS = [ + "thumb_cmc", + "index_dip", + "middle_mcp_abduction", + "middle_dip", + "pinky_mcp_abduction", + "pinky_dip", +] + +RIGHT_HAND_INVERSIONS = [ + "thumb_mcp", + "thumb_cmc", + "index_mcp_abduction", + "index_dip", + "middle_mcp_abduction", + "middle_dip", + "ring_mcp_abduction", + "ring_mcp_flexion", + "ring_dip", + "pinky_mcp_abduction", +] + class HomonculusGlove(Teleoperator): """ @@ -62,14 +84,7 @@ class HomonculusGlove(Teleoperator): "pinky_mcp_flexion": MotorNormMode.RANGE_0_100, "pinky_dip": MotorNormMode.RANGE_0_100, } - self.inverted_joints = [ - "thumb_cmc", - "index_dip", - "middle_mcp_abduction", - "middle_dip", - "pinky_mcp_abduction", - "pinky_dip", - ] + self.inverted_joints = RIGHT_HAND_INVERSIONS if config.side == "right" else LEFT_HAND_INVERSIONS self._state: dict[str, float] | None = None self.new_state_event = threading.Event() @@ -158,6 +173,8 @@ class HomonculusGlove(Teleoperator): elif not isinstance(joints, list): raise TypeError(joints) + display_len = max(len(key) for key in joints) + start_positions = self._read(joints, normalize=False) mins = start_positions.copy() maxes = start_positions.copy() @@ -168,9 +185,11 @@ class HomonculusGlove(Teleoperator): if display_values: print("\n-------------------------------------------") - print(f"{'NAME':<15} | {'MIN':>6} | {'POS':>6} | {'MAX':>6}") + print(f"{'NAME':<{display_len}} | {'MIN':>6} | {'POS':>6} | {'MAX':>6}") for joint in joints: - print(f"{joint:<15} | {mins[joint]:>6} | {positions[joint]:>6} | {maxes[joint]:>6}") + print( + f"{joint:<{display_len}} | {mins[joint]:>6} | {positions[joint]:>6} | {maxes[joint]:>6}" + ) if enter_pressed(): break