diff --git a/lelab/teleoperate.py b/lelab/teleoperate.py index 63416803..a7b9bc50 100644 --- a/lelab/teleoperate.py +++ b/lelab/teleoperate.py @@ -28,25 +28,6 @@ logger = logging.getLogger(__name__) -# sts3215 motor resolution; lerobot's _normalize uses (resolution - 1). -_STS3215_MAX_RES = 4095 - -# SO-101 URDF (so101_new_calib.urdf) is authored with the all-zeros pose at the -# arm's sleep position, not the "middle of range" pose where calibration's -# set_half_turn_homings is performed. To make the URDF track the real arm: -# URDF_value = sign * (motor_normalized_deg - motor_at_urdf_zero_deg) -# where motor_at_urdf_zero_deg = (urdf_zero_ticks - mid) * 360 / max_res, and -# `urdf_zero_ticks` is the raw Present_Position when the robot is at sleep. -# That tick value is a property of the SO-101 mechanics + URDF design, so it's -# constant across calibrations as long as the user pressed ENTER at the "middle -# of range" pose during set_half_turn_homings. -# Joints not listed here use lerobot's default convention (URDF = motor). -_SO101_URDF_CORRECTIONS = { - # motor_name: (sign, urdf_zero_present_position_ticks) - "shoulder_lift": (+1, 3252), - "elbow_flex": (+1, 1029), -} - # Global variables for teleoperation state teleoperation_active = False teleoperation_thread: threading.Thread | None = None @@ -67,6 +48,13 @@ def get_joint_positions_from_robot(robot) -> dict[str, float]: """ Extract current joint positions from the robot and convert to URDF joint format. + lerobot drives the SO-101 with ``use_degrees=True`` by default, so each + ``observation[".pos"]`` is already the joint angle in degrees relative + to the calibration center — which is also the URDF's zero pose. The URDF + joint value is therefore just that angle converted to radians, for every + joint. (The gripper reports 0–100 rather than degrees, but that range lands + inside the Jaw limit, matching the open/closed sweep.) + Args: robot: The robot instance (SO101Follower) @@ -84,7 +72,6 @@ def get_joint_positions_from_robot(robot) -> dict[str, float]: try: observation = robot.get_observation() - calibration = robot.calibration or {} joint_positions: dict[str, float] = {} debug_rows = [] @@ -95,20 +82,9 @@ def get_joint_positions_from_robot(robot) -> dict[str, float]: joint_positions[urdf_joint_name] = 0.0 continue - raw_deg = observation[motor_key] - angle_degrees = raw_deg - correction = _SO101_URDF_CORRECTIONS.get(motor_name) - if correction is not None and motor_name in calibration: - sign, urdf_zero_ticks = correction - cal = calibration[motor_name] - mid = (cal.range_min + cal.range_max) / 2 - motor_at_urdf_zero = (urdf_zero_ticks - mid) * 360 / _STS3215_MAX_RES - angle_degrees = sign * (raw_deg - motor_at_urdf_zero) - + angle_degrees = observation[motor_key] joint_positions[urdf_joint_name] = angle_degrees * math.pi / 180.0 - debug_rows.append( - f"{motor_name:14s} raw={raw_deg:+8.2f}° → {urdf_joint_name:11s} = {angle_degrees:+8.2f}°" - ) + debug_rows.append(f"{motor_name:14s} {angle_degrees:+8.2f}° → {urdf_joint_name:11s}") # Throttled debug print (~once per second at 20 Hz broadcast). now = time.time() diff --git a/tests/test_teleoperate.py b/tests/test_teleoperate.py index 57a84818..652a2f40 100644 --- a/tests/test_teleoperate.py +++ b/tests/test_teleoperate.py @@ -51,6 +51,44 @@ def test_get_joint_positions_from_robot_uses_provided_object() -> None: assert isinstance(positions, dict) +def test_get_joint_positions_maps_degrees_to_radians_without_correction() -> None: + """Every joint maps its Present_Position (degrees around the calibration + center) straight to radians, deg * pi/180, with no per-joint offset. + shoulder_lift/elbow_flex used to be run through a correction table; a + calibration that would have triggered it is supplied here to prove the + offset is gone and they now map like every other joint.""" + import math + + from lelab.teleoperate import get_joint_positions_from_robot + + class _Cal: + # The removed correction derived its offset from range_min/range_max. + range_min = 0 + range_max = 4095 + + class _Robot: + calibration = {"shoulder_lift": _Cal(), "elbow_flex": _Cal()} + + def get_observation(self): + return { + "shoulder_pan.pos": 0.0, + "shoulder_lift.pos": 90.0, + "elbow_flex.pos": -45.0, + "wrist_flex.pos": 30.0, + "wrist_roll.pos": 12.0, + "gripper.pos": 50.0, + } + + positions = get_joint_positions_from_robot(_Robot()) + + assert positions["Rotation"] == pytest.approx(0.0) + assert positions["Pitch"] == pytest.approx(math.radians(90.0)) + assert positions["Elbow"] == pytest.approx(math.radians(-45.0)) + assert positions["Wrist_Pitch"] == pytest.approx(math.radians(30.0)) + assert positions["Wrist_Roll"] == pytest.approx(math.radians(12.0)) + assert positions["Jaw"] == pytest.approx(math.radians(50.0)) + + def test_start_teleoperation_reports_connection_failure( monkeypatch: pytest.MonkeyPatch, ) -> None: