fix(teleop): stop sim joints 2 and 3 pinning in the 3D viewer - #86
Open
cn0303 wants to merge 2 commits into
Open
fix(teleop): stop sim joints 2 and 3 pinning in the 3D viewer#86cn0303 wants to merge 2 commits into
cn0303 wants to merge 2 commits into
Conversation
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.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
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.