fix(teleop): stop sim joints 2 and 3 pinning in the 3D viewer - #86
Conversation
nicolas-rabault
left a comment
There was a problem hiding this comment.
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.
| angle_degrees = observation[motor_key] | ||
| joint_positions[urdf_joint_name] = angle_degrees * math.pi / 180.0 |
There was a problem hiding this comment.
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.
Dismissing my own review. On a second pass none of these block merge, see the comment below.
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.
4b2ffc6 to
3f655e1
Compare
What does this PR do?
During teleop the 3D URDF viewer's
shoulder_lift(Pitch) andelbow_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_CORRECTIONStable that ran only those two jointsthrough a
motor_at_urdf_zerooffset of about 110 degrees before converting toradians. That offset pushed the value past the URDF's symmetric limit of about
100 degrees, where
urdf-loader'ssetJointValueclamps it, so the jointpinned. 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) defaultuse_degrees=True, soobservation["<motor>.pos"]is already the joint angle in degrees about thecalibration 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: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.