diff --git a/src/lerobot/robots/unitree_g1/controllers/sonic_pipeline.py b/src/lerobot/robots/unitree_g1/controllers/sonic_pipeline.py index 329a28d85..0ba11cbeb 100644 --- a/src/lerobot/robots/unitree_g1/controllers/sonic_pipeline.py +++ b/src/lerobot/robots/unitree_g1/controllers/sonic_pipeline.py @@ -253,6 +253,7 @@ def compute_kp_kd(): Ankle and waist joints get a x2 factor for extra stiffness. Returns two (29,) float32 arrays in IsaacLab joint order. """ + def s(k): return ARMATURE[k] * NATURAL_FREQ**2 diff --git a/src/lerobot/teleoperators/pico_headset/smpl_fk.py b/src/lerobot/teleoperators/pico_headset/smpl_fk.py index 5dcf96786..bd28f7e75 100644 --- a/src/lerobot/teleoperators/pico_headset/smpl_fk.py +++ b/src/lerobot/teleoperators/pico_headset/smpl_fk.py @@ -49,9 +49,63 @@ OUTPUT_JOINT_INDEX = np.concatenate([np.arange(22), np.array([39, 54])]) # asset. ``_SKELETON_PARENTS[i]`` is joint ``i``'s parent (-1 = root); ``_SKELETON_J`` # holds the (55, 3) rest-pose joint positions. _SKELETON_PARENTS = np.array( - [-1, 0, 0, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 9, 9, 12, 13, 14, 16, 17, 18, 19, 15, 15, 15, - 20, 25, 26, 20, 28, 29, 20, 31, 32, 20, 34, 35, 20, 37, 38, 21, 40, 41, 21, 43, 44, - 21, 46, 47, 21, 49, 50, 21, 52, 53], + [ + -1, + 0, + 0, + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 9, + 9, + 12, + 13, + 14, + 16, + 17, + 18, + 19, + 15, + 15, + 15, + 20, + 25, + 26, + 20, + 28, + 29, + 20, + 31, + 32, + 20, + 34, + 35, + 20, + 37, + 38, + 21, + 40, + 41, + 21, + 43, + 44, + 21, + 46, + 47, + 21, + 49, + 50, + 21, + 52, + 53, + ], dtype=np.int64, )