diff --git a/lerobot/common/motors/motors_bus.py b/lerobot/common/motors/motors_bus.py index 82a69cef3..aa53dc97d 100644 --- a/lerobot/common/motors/motors_bus.py +++ b/lerobot/common/motors/motors_bus.py @@ -786,13 +786,19 @@ class MotorsBus(abc.ABC): raise ValueError(f"Invalid calibration for motor '{motor}': min and max are equal.") bounded_val = min(max_, max(min_, val)) + # TODO(Steven): normalization can go boom if max_ == min_, we should add a check probably in record_ranges_of_motions + # (which probably indicates the user forgot to move a motor, most likely a gripper-like one) if self.motors[motor].norm_mode is MotorNormMode.RANGE_M100_100: norm = (((bounded_val - min_) / (max_ - min_)) * 200) - 100 normalized_values[id_] = -norm if drive_mode else norm elif self.motors[motor].norm_mode is MotorNormMode.RANGE_0_100: norm = ((bounded_val - min_) / (max_ - min_)) * 100 normalized_values[id_] = 100 - norm if drive_mode else norm - else: + elif self.motors[motor].norm_mode is MotorNormMode.DEGREE: + if drive_mode: + val *= -1 + val += homing_offset + normalized_values[id_] = val / (self.model_resolution_table[self.motors[motor].model] // 2) * 180 # TODO(alibers): degree mode raise NotImplementedError @@ -819,6 +825,12 @@ class MotorsBus(abc.ABC): val = 100 - val if drive_mode else val bounded_val = min(100.0, max(0.0, val)) unnormalized_values[id_] = int((bounded_val / 100) * (max_ - min_) + min_) + elif self.motors[motor].norm_mode is MotorNormMode.DEGREE: + homing_offset = self.calibration[motor].homing_offset + unnormalized_values[id_] = int(val / 180 * (self.model_resolution_table[self.motors[motor].model] // 2)) + unnormalized_values[id_] -= homing_offset + if drive_mode: + unnormalized_values[id_] *= -1 else: # TODO(aliberts): degree mode raise NotImplementedError diff --git a/lerobot/common/robots/__init__.py b/lerobot/common/robots/__init__.py index d8fd0de93..c05cad250 100644 --- a/lerobot/common/robots/__init__.py +++ b/lerobot/common/robots/__init__.py @@ -1,3 +1,4 @@ from .config import RobotConfig from .robot import Robot +from .robot_wrapper import RobotWrapper from .utils import make_robot_from_config diff --git a/lerobot/common/robots/utils.py b/lerobot/common/robots/utils.py index ec0374955..587d75246 100644 --- a/lerobot/common/robots/utils.py +++ b/lerobot/common/robots/utils.py @@ -14,10 +14,40 @@ import logging from pprint import pformat +from typing import Type from lerobot.common.robots import RobotConfig from .robot import Robot +from .robot_wrapper import RobotWrapper + + +def make_robot_config(robot_type: str, **kwargs) -> RobotConfig: + if robot_type == "aloha": + raise NotImplementedError # TODO + + elif robot_type == "koch_follower": + from .koch_follower.config_koch_follower import KochFollowerConfig + + return KochFollowerConfig(**kwargs) + elif robot_type == "so100_follower": + from .so100_follower.config_so100_follower import SO100FollowerConfig + + return SO100FollowerConfig(**kwargs) + elif robot_type == "so100_follower_end_effector": + from .so100_follower_end_effector.config_so100_follower_end_effector import SO100FollowerEndEffectorConfig + + return SO100FollowerEndEffectorConfig(**kwargs) + elif robot_type == "stretch": + from .stretch3.configuration_stretch3 import Stretch3RobotConfig + + return Stretch3RobotConfig(**kwargs) + elif robot_type == "lekiwi": + from .lekiwi.config_lekiwi import LeKiwiConfig + + return LeKiwiConfig(**kwargs) + else: + raise ValueError(f"Robot type '{robot_type}' is not available.") def make_robot_from_config(config: RobotConfig) -> Robot: @@ -29,6 +59,10 @@ def make_robot_from_config(config: RobotConfig) -> Robot: from .so100_follower import SO100Follower return SO100Follower(config) + elif config.type == "so100_follower_end_effector": + from .so100_follower_end_effector import SO100FollowerEndEffector + + return SO100FollowerEndEffector(config) elif config.type == "so101_follower": from .so101_follower import SO101Follower @@ -97,3 +131,9 @@ def get_arm_id(name, arm_type): like Aloha, it could be left_follower, right_follower, left_leader, or right_leader. """ return f"{name}_{arm_type}" + +def convert_joint_m100_100_to_degrees(joint_positions: dict[str, float], mins: dict[str, float], maxs: dict[str, float]) -> dict[str, float]: + return {key: ((value + 100) / 200) * (maxs[key] - mins[key]) + mins[key] for key, value in joint_positions.items()} + +def convert_joint_degrees_to_m100_100(joint_positions: dict[str, float], mins: dict[str, float], maxs: dict[str, float]) -> dict[str, float]: + return {key: (value - mins[key]) / (maxs[key] - mins[key]) * 200 - 100 for key, value in joint_positions.items()} diff --git a/lerobot/common/teleoperators/utils.py b/lerobot/common/teleoperators/utils.py index 4942084ac..d7b7bcf0e 100644 --- a/lerobot/common/teleoperators/utils.py +++ b/lerobot/common/teleoperators/utils.py @@ -45,5 +45,9 @@ def make_teleoperator_from_config(config: TeleoperatorConfig) -> Teleoperator: from tests.mocks.mock_teleop import MockTeleop return MockTeleop(config) + elif config.type == "gamepad": + from .gamepad.teleop_gamepad import GamepadTeleop + + return GamepadTeleop(config) else: raise ValueError(config.type) diff --git a/lerobot/common/utils/utils.py b/lerobot/common/utils/utils.py index f45ad0e46..d9f7f78db 100644 --- a/lerobot/common/utils/utils.py +++ b/lerobot/common/utils/utils.py @@ -19,6 +19,8 @@ import os.path as osp import platform import subprocess import time +import select +import sys from copy import copy, deepcopy from datetime import datetime, timezone from pathlib import Path @@ -244,6 +246,14 @@ def is_valid_numpy_dtype_string(dtype_str: str) -> bool: return False +def enter_pressed() -> bool: + return select.select([sys.stdin], [], [], 0)[0] and sys.stdin.readline().strip() == "" + + +def move_cursor_up(lines): + """Move the cursor up by a specified number of lines.""" + print(f"\033[{lines}A", end="") + class TimerManager: """ Lightweight utility to measure elapsed time. diff --git a/lerobot/scripts/server/end_effector_control_utils.py b/lerobot/scripts/server/end_effector_control_utils.py index d863fd223..74003809d 100644 --- a/lerobot/scripts/server/end_effector_control_utils.py +++ b/lerobot/scripts/server/end_effector_control_utils.py @@ -22,7 +22,7 @@ import time import numpy as np import torch -from lerobot.common.robot_devices.utils import busy_wait +from lerobot.common.utils.robot_utils import busy_wait from lerobot.common.utils.utils import init_logging from lerobot.scripts.server.kinematics import RobotKinematics @@ -95,7 +95,7 @@ class InputController: def gripper_command(self): """Return the current gripper command.""" if self.open_gripper_command == self.close_gripper_command: - return "no-op" + return "stay" elif self.open_gripper_command: return "open" elif self.close_gripper_command: diff --git a/lerobot/teleoperate.py b/lerobot/teleoperate.py index 84acfdc5b..a8c6e4f6d 100644 --- a/lerobot/teleoperate.py +++ b/lerobot/teleoperate.py @@ -47,6 +47,7 @@ from lerobot.common.robots import ( # noqa: F401 koch_follower, make_robot_from_config, so100_follower, + so100_follower_end_effector, so101_follower, ) from lerobot.common.teleoperators import ( @@ -57,7 +58,7 @@ from lerobot.common.teleoperators import ( from lerobot.common.utils.utils import init_logging, move_cursor_up from lerobot.common.utils.visualization_utils import _init_rerun -from .common.teleoperators import koch_leader, so100_leader, so101_leader # noqa: F401 +from .common.teleoperators import koch_leader, so100_leader, gamepad # noqa: F401 @dataclass