Skip to content

fix(teleop): stop sim joints 2 and 3 pinning in the 3D viewer - #86

Merged
nicolas-rabault merged 2 commits into
huggingface:mainfrom
cn0303:fix/teleop-urdf-joint-mapping
Aug 3, 2026
Merged

fix(teleop): stop sim joints 2 and 3 pinning in the 3D viewer#86
nicolas-rabault merged 2 commits into
huggingface:mainfrom
cn0303:fix/teleop-urdf-joint-mapping

Conversation

@cn0303

@cn0303 cn0303 commented Jul 25, 2026

Copy link
Copy Markdown
Contributor

What does this PR do?

During teleop the 3D URDF viewer's shoulder_lift (Pitch) and elbow_flex
(Elbow) joints pinned at their limits and stopped tracking the real arm. The
other four joints tracked fine.

The cause was a _SO101_URDF_CORRECTIONS table that ran only those two joints
through a motor_at_urdf_zero offset of about 110 degrees before converting to
radians. That offset pushed the value past the URDF's symmetric limit of about
100 degrees, where urdf-loader's setJointValue clamps it, so the joint
pinned. A live trace confirmed it: shoulder_lift -101 -> Pitch -211 (clamped to
-100), elbow_flex +97 -> Elbow +186 (clamped to +90).

The fix

SO101FollowerConfig (and the leader) default use_degrees=True, so
observation["<motor>.pos"] is already the joint angle in degrees about the
calibration center, which is the same zero the URDF is authored around. The four
joints that always tracked prove it: they map degrees to radians directly with no
offset. So the fix deletes _SO101_URDF_CORRECTIONS (and the now-unused
_STS3215_MAX_RES) and maps every joint the same way:

angle_degrees = observation[motor_key]
joint_positions[urdf_joint_name] = angle_degrees * math.pi / 180.0

The gripper is unchanged (its 0 to 100 range already lands inside the Jaw limit).
The throttled [joint-debug] log is kept as a live verification aid.

Testing

ruff check and ruff format green. Backend imports cleanly. pytest: 198 passed;
the only 3 failures are pre-existing on main and unrelated to teleop (Windows
dataset_repair timestamp and two jobs pid/checkpoint tests), confirmed by running
them against a clean main.

Hardware-verified on Windows 11 (SO-101 leader plus follower, lerobot 0.6.0):
moving the leader through its range, Pitch and Elbow track the real arm in the 3D
viewer and no longer pin, matching the other four joints, with no sign flip.

@nicolas-rabault
nicolas-rabault self-requested a review August 3, 2026 12:20

@nicolas-rabault nicolas-rabault left a comment

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for the live trace in the description, that is what made this quick to check. I verified the diagnosis independently before reading your reasoning, and it holds up.

What I checked and what held:

SOFollowerConfig.use_degrees really does default to True at the pinned lerobot v0.6.0, and leLab never overrides it in teleoperate.py. So observation[".pos"] is (ticks - mid) * 360 / 4095, degrees about the calibration center, and gripper is RANGE_0_100, exactly as your docstring says. Jaw's upper limit is 1.74533 rad, which is 100.0 degrees, so the 0 to 100 gripper range lands inside it by construction.
The URDF settles the removed table's premise. Running forward kinematics on so101_new_calib.urdf with every joint at zero puts the upper arm vertical and the forearm horizontal forward, gripper about 30 cm out. That is a middle-of-range pose, not the sleep pose the old comment assumed. The declared limits agree: Pitch is symmetric at +/- 100 degrees, which is shoulder_lift's travel about the calibration center almost exactly.
The old offset worked out to about 106 degrees, so shoulder_lift at the calibration center mapped to about -106, already past the +/- 100 limit. Under that table most of the arm's travel was unreachable in the sim, so the correction could not have been right under any zero convention.
Blast radius is contained. _STS3215_MAX_RES, _SO101_URDF_CORRECTIONS and robot.calibration had no other reader, get_joint_positions_from_robot is the only place motor values become URDF joints, and the frontend hands the radians straight to setJointValue with no offset of its own. Nothing that commands hardware reads this.
ruff check and ruff format --check are clean and the full suite is green on 4b2ffc6 locally, 202 passed. The empty check list on this PR is not a failure: fork workflows on this repo sit in action_required until they are approved.
Three things before it lands, all inline and none of them large. The one that matters is Elbow, whose URDF limit is asymmetric, so the pinning is not quite fully gone.

Comment thread lelab/teleoperate.py
Comment on lines +85 to 86
angle_degrees = observation[motor_key]
joint_positions[urdf_joint_name] = angle_degrees * math.pi / 180.0

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Elbow is the one joint whose URDF limit is not symmetric: so101_new_calib.urdf declares lower="-1.74533" upper="1.5708", so -100 to +90 degrees. Your own trace has elbow_flex reporting +97, which the direct mapping still clamps at +90. So the fix is complete for Pitch, but Elbow can still pin in roughly the last 7 degrees of flexion.

Nothing to change on this line, it is the right mapping. I would just like the intent stated: either accept it as a fidelity limit of the shipped URDF and say so in the description, or widen Elbow's upper limit to match the motor's travel. Both are fine, I would rather the description matched the behaviour.

@nicolas-rabault
nicolas-rabault dismissed their stale review August 3, 2026 12:50

Dismissing my own review. On a second pass none of these block merge, see the comment below.

cn0303 added 2 commits August 3, 2026 14:52
shoulder_lift (Pitch) and elbow_flex (Elbow) pinned at the URDF limits in the
3D sim and stopped tracking the real arm. The _SO101_URDF_CORRECTIONS table
subtracted a ~110° "URDF zero" offset from just these two joints, which shoved
their value past the joint's ±100° URDF limit, so urdf-loader clamped them
(live trace: shoulder_lift +/-101° -> Pitch -211°, clamped to -100°).

lerobot drives the SO-101 with use_degrees=True by default, so each
observation["<motor>.pos"] is already the joint angle in degrees about the
calibration center — the same zero the URDF is authored around. The four
joints that always tracked correctly prove the point: they map degrees
straight to radians with no offset. shoulder_lift and elbow_flex need the same
treatment, not a special correction. Drop the table so every joint maps
identically.
Add a unit test that feeds a fake observation and asserts every joint maps
straight to radians (deg * pi/180). It supplies a calibration that would have
triggered the removed shoulder_lift/elbow_flex correction, so it fails against
the old offset table and passes now that all joints map identically.
@nicolas-rabault
nicolas-rabault force-pushed the fix/teleop-urdf-joint-mapping branch from 4b2ffc6 to 3f655e1 Compare August 3, 2026 12:52
@nicolas-rabault
nicolas-rabault merged commit 8cabe07 into huggingface:main Aug 3, 2026
2 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants