tangger/lerobot
1
0
Fork 1
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

precomit nits

Browse Source
This commit is contained in:
Michel Aractingi
2025-05-20 21:20:57 +02:00
committed by AdilZouitine
parent ab85147296
commit ba477e81ce
7 changed files with 48 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
lerobot/common/model/kinematics.py
View File

@@ -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'}")
print(f"\nOverall: {'ALL TESTS PASSED' if all_passed else 'SOME TESTS FAILED'}")

2
lerobot/common/robots/so100_follower_end_effector/__init__.py
View File

@@ -1,2 +1,2 @@
from .config_so100_follower_end_effector import SO100FollowerEndEffectorConfig
from .so100_follower_end_effector import SO100FollowerEndEffector
from .so100_follower_end_effector import SO100FollowerEndEffector

16
lerobot/common/robots/so100_follower_end_effector/config_so100_follower_end_effector.py
View File

@@ -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
} )
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
}
)

52
lerobot/common/robots/so100_follower_end_effector/so100_follower_end_effector.py
View File

@@ -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)
return super().send_action(joint_action)

1
lerobot/common/robots/utils.py
View File

@@ -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}"

5
lerobot/common/teleoperators/gamepad/gamepad_utils.py
View File

@@ -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:

10
lerobot/common/teleoperators/gamepad/teleop_gamepad.py
View File

@@ -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(