Update joint visualization spheres using body positions
(viewer, motion_loader, t)
| 266 | |
| 267 | |
| 268 | def update_joint_visualization(viewer, motion_loader, t): |
| 269 | """Update joint visualization spheres using body positions""" |
| 270 | if t < 0 or t >= motion_loader.T: |
| 271 | return |
| 272 | |
| 273 | # Get body positions from motion data |
| 274 | body_pos = motion_loader.body_pos[t] # [N, 3] |
| 275 | |
| 276 | # Update spheres for each body (excluding the first one which is pelvis) |
| 277 | for i in range(0, len(body_pos)): |
| 278 | viewer.add_marker( |
| 279 | pos=body_pos[i], |
| 280 | size=[0.02, 0.02, 0.02], |
| 281 | rgba=[0.8, 0.5, 0.3, 1], |
| 282 | type=mujoco.mjtGeom.mjGEOM_SPHERE, |
| 283 | label="" |
| 284 | ) |
| 285 | |
| 286 | # Update anchor body sphere (larger) - use torso_link position |
| 287 | anchor_pos = body_pos[motion_loader.anchor_body_index] # torso_link position |
| 288 | viewer.add_marker( |
| 289 | pos=anchor_pos, size=[0.05, 0.05, 0.05], rgba=[0.8, 0.5, 0.3, 1], type=mujoco.mjtGeom.mjGEOM_SPHERE, label="" |
| 290 | ) |
| 291 | |
| 292 | |
| 293 | # ====== MotionLoader 类（参考 isaaclab） ====== |