From ba477e81ceab317e1ef8ae05f497368ef63ce85e Mon Sep 17 00:00:00 2001 From: Michel Aractingi Date: Tue, 20 May 2025 21:20:57 +0200 Subject: [PATCH] precomit nits --- lerobot/common/model/kinematics.py | 2 +- .../so100_follower_end_effector/__init__.py | 2 +- .../config_so100_follower_end_effector.py | 16 +++--- .../so100_follower_end_effector.py | 52 +++++++++++-------- lerobot/common/robots/utils.py | 1 - .../teleoperators/gamepad/gamepad_utils.py | 5 +- .../teleoperators/gamepad/teleop_gamepad.py | 10 ++-- 7 files changed, 48 insertions(+), 40 deletions(-) diff --git a/lerobot/common/model/kinematics.py b/lerobot/common/model/kinematics.py index c01ad0fe9..c42d9b2fd 100644 --- a/lerobot/common/model/kinematics.py +++ b/lerobot/common/model/kinematics.py @@ -543,4 +543,4 @@ if __name__ == "__main__": all_passed = all(results.values()) for robot_type, passed in results.items(): print(f"{robot_type.upper()}: {'PASSED' if passed else 'FAILED'}") - print(f"\nOverall: {'ALL TESTS PASSED' if all_passed else 'SOME TESTS FAILED'}") \ No newline at end of file + print(f"\nOverall: {'ALL TESTS PASSED' if all_passed else 'SOME TESTS FAILED'}") diff --git a/lerobot/common/robots/so100_follower_end_effector/__init__.py b/lerobot/common/robots/so100_follower_end_effector/__init__.py index 7df379018..726e56040 100644 --- a/lerobot/common/robots/so100_follower_end_effector/__init__.py +++ b/lerobot/common/robots/so100_follower_end_effector/__init__.py @@ -1,2 +1,2 @@ from .config_so100_follower_end_effector import SO100FollowerEndEffectorConfig -from .so100_follower_end_effector import SO100FollowerEndEffector \ No newline at end of file +from .so100_follower_end_effector import SO100FollowerEndEffector diff --git a/lerobot/common/robots/so100_follower_end_effector/config_so100_follower_end_effector.py b/lerobot/common/robots/so100_follower_end_effector/config_so100_follower_end_effector.py index e828101f7..6f0f8c54d 100644 --- a/lerobot/common/robots/so100_follower_end_effector/config_so100_follower_end_effector.py +++ b/lerobot/common/robots/so100_follower_end_effector/config_so100_follower_end_effector.py @@ -14,10 +14,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Dict, List, Optional - from dataclasses import dataclass, field +from typing import Dict, List + from lerobot.common.cameras import CameraConfig + from ..config import RobotConfig @@ -25,6 +26,7 @@ from ..config import RobotConfig @dataclass class SO100FollowerEndEffectorConfig(RobotConfig): """Configuration for the SO100FollowerEndEffector robot.""" + # Port to connect to the arm port: str @@ -39,7 +41,9 @@ class SO100FollowerEndEffectorConfig(RobotConfig): cameras: dict[str, CameraConfig] = field(default_factory=dict) # Default bounds for the end-effector position (in meters) - bounds: Dict[str, List[float]] = field(default_factory=lambda: { - "min": [0.1, -0.3, 0.0], # min x, y, z - "max": [0.4, 0.3, 0.4], # max x, y, z - } ) \ No newline at end of file + bounds: Dict[str, List[float]] = field( + default_factory=lambda: { + "min": [0.1, -0.3, 0.0], # min x, y, z + "max": [0.4, 0.3, 0.4], # max x, y, z + } + ) diff --git a/lerobot/common/robots/so100_follower_end_effector/so100_follower_end_effector.py b/lerobot/common/robots/so100_follower_end_effector/so100_follower_end_effector.py index 9a691f3f0..694c35427 100644 --- a/lerobot/common/robots/so100_follower_end_effector/so100_follower_end_effector.py +++ b/lerobot/common/robots/so100_follower_end_effector/so100_follower_end_effector.py @@ -15,16 +15,17 @@ # limitations under the License. import logging -import numpy as np from typing import Any, Dict +import numpy as np + from lerobot.common.errors import DeviceNotConnectedError from lerobot.common.model.kinematics import RobotKinematics +from lerobot.common.motors import Motor, MotorNormMode +from lerobot.common.motors.feetech import FeetechMotorsBus from ..so100_follower import SO100Follower from .config_so100_follower_end_effector import SO100FollowerEndEffectorConfig -from lerobot.common.motors import Motor, MotorNormMode -from lerobot.common.motors.feetech import FeetechMotorsBus logger = logging.getLogger(__name__) @@ -32,7 +33,7 @@ logger = logging.getLogger(__name__) class SO100FollowerEndEffector(SO100Follower): """ SO100Follower robot with end-effector space control. - + This robot inherits from SO100Follower but transforms actions from end-effector space to joint space before sending them to the motors. """ @@ -56,13 +57,13 @@ class SO100FollowerEndEffector(SO100Follower): ) self.config = config - + # Initialize the kinematics module for the so100 robot self.kinematics = RobotKinematics(robot_type="so100") - + # Set the forward kinematics function self.fk_function = self.kinematics.fk_gripper_tip - + # Store the bounds for end-effector position self.bounds = self.config.bounds @@ -90,26 +91,30 @@ class SO100FollowerEndEffector(SO100Follower): def send_action(self, action: Dict[str, Any]) -> Dict[str, Any]: """ Transform action from end-effector space to joint space and send to motors. - + Args: action: Dictionary with keys 'delta_x', 'delta_y', 'delta_z' for end-effector control or a numpy array with [delta_x, delta_y, delta_z] - + Returns: The joint-space action that was sent to the motors """ if not self.is_connected: raise DeviceNotConnectedError(f"{self} is not connected.") - + # Convert action to numpy array if not already if isinstance(action, dict): if all(k in action for k in ["delta_x", "delta_y", "delta_z", "gripper"]): - action = np.array([action["delta_x"], action["delta_y"], action["delta_z"], action["gripper"]], dtype=np.float32) + action = np.array( + [action["delta_x"], action["delta_y"], action["delta_z"], action["gripper"]], + dtype=np.float32, + ) else: - logger.warning(f"Expected action keys 'delta_x', 'delta_y', 'delta_z', got {list(action.keys())}") + logger.warning( + f"Expected action keys 'delta_x', 'delta_y', 'delta_z', got {list(action.keys())}" + ) action = np.zeros(4, dtype=np.float32) - # Read current joint positions current_joint_pos = self.bus.sync_read("Present_Position") @@ -117,14 +122,14 @@ class SO100FollowerEndEffector(SO100Follower): joint_names = ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"] # Convert the joint positions from min-max to degrees current_joint_pos = np.array([current_joint_pos[name] for name in joint_names]) - + # Calculate current end-effector position using forward kinematics current_ee_pos = self.fk_function(current_joint_pos) - + # Set desired end-effector position by adding delta desired_ee_pos = np.eye(4) desired_ee_pos[:3, :3] = current_ee_pos[:3, :3] # Keep orientation - + # Add delta to position and clip to bounds desired_ee_pos[:3, 3] = current_ee_pos[:3, 3] + action[:3] if self.bounds is not None: @@ -133,7 +138,7 @@ class SO100FollowerEndEffector(SO100Follower): self.bounds["min"], self.bounds["max"], ) - + # Compute inverse kinematics to get joint positions target_joint_values_in_degrees = self.kinematics.ik( current_joint_pos, @@ -141,11 +146,11 @@ class SO100FollowerEndEffector(SO100Follower): position_only=True, fk_func=self.fk_function, ) - + # Create joint space action dictionary joint_action = { - f"{joint_names[i]}.pos": target_joint_values_in_degrees[i] - for i in range(len(joint_names)-1) # Exclude gripper + f"{joint_names[i]}.pos": target_joint_values_in_degrees[i] + for i in range(len(joint_names) - 1) # Exclude gripper } # Handle gripper separately if included in action @@ -154,8 +159,9 @@ class SO100FollowerEndEffector(SO100Follower): else: # Keep current gripper position joint_action["gripper.pos"] = current_joint_pos[-1] - - import time + + import time + time.sleep(0.001) # Send joint space action to parent class - return super().send_action(joint_action) \ No newline at end of file + return super().send_action(joint_action) diff --git a/lerobot/common/robots/utils.py b/lerobot/common/robots/utils.py index dd1002251..b574835ec 100644 --- a/lerobot/common/robots/utils.py +++ b/lerobot/common/robots/utils.py @@ -131,4 +131,3 @@ 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}" - diff --git a/lerobot/common/teleoperators/gamepad/gamepad_utils.py b/lerobot/common/teleoperators/gamepad/gamepad_utils.py index cb9ba0b1f..f2a70f590 100644 --- a/lerobot/common/teleoperators/gamepad/gamepad_utils.py +++ b/lerobot/common/teleoperators/gamepad/gamepad_utils.py @@ -14,17 +14,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -import argparse import logging -import sys import time import numpy as np import torch -from lerobot.common.utils.robot_utils import busy_wait -from lerobot.common.utils.utils import init_logging from lerobot.common.robots.kinematics import RobotKinematics +from lerobot.common.utils.robot_utils import busy_wait class InputController: diff --git a/lerobot/common/teleoperators/gamepad/teleop_gamepad.py b/lerobot/common/teleoperators/gamepad/teleop_gamepad.py index ace9926ec..5d4a5cafd 100644 --- a/lerobot/common/teleoperators/gamepad/teleop_gamepad.py +++ b/lerobot/common/teleoperators/gamepad/teleop_gamepad.py @@ -15,20 +15,22 @@ # limitations under the License. import sys +from enum import IntEnum from queue import Queue from typing import Any -from enum import IntEnum -from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError import numpy as np + from ..teleoperator import Teleoperator from .configuration_gamepad import GamepadTeleopConfig + class GripperAction(IntEnum): CLOSE = 0 STAY = 1 OPEN = 2 + gripper_action_map = { "close": GripperAction.CLOSE.value, "open": GripperAction.OPEN.value, @@ -58,7 +60,7 @@ class GamepadTeleop(Teleoperator): @property def action_features(self) -> dict: - if self.config.use_gripper: + if self.config.use_gripper: return { "dtype": "float32", "shape": (4,), @@ -88,7 +90,7 @@ class GamepadTeleop(Teleoperator): if sys.platform == "darwin": # NOTE: On macOS, pygame doesn’t reliably detect input from some controllers so we fall back to hidapi from lerobot.scripts.server.end_effector_control_utils import GamepadControllerHID as Gamepad - else: + else: from lerobot.scripts.server.end_effector_control_utils import GamepadController as Gamepad self.gamepad = Gamepad(