Get command (joint_pos + joint_vel) for future steps - matching Isaac Lab order
(motion_loader, t)
| 364 | |
| 365 | # ====== observation 计算函数 ====== |
| 366 | def get_command(motion_loader, t): |
| 367 | """Get command (joint_pos + joint_vel) for future steps - matching Isaac Lab order""" |
| 368 | if t < 0: |
| 369 | return np.zeros(290, dtype=np.float32) # 29 * 2 * 5 = 290 |
| 370 | |
| 371 | # Get future joint positions and velocities - batch process like Isaac Lab |
| 372 | future_joint_pos = [] |
| 373 | future_joint_vel = [] |
| 374 | |
| 375 | for i in range(motion_loader.future_steps): |
| 376 | step_idx = min(t + i, motion_loader.T - 1) |
| 377 | future_joint_pos.append(motion_loader.joint_pos[step_idx]) |
| 378 | future_joint_vel.append(motion_loader.joint_vel[step_idx]) |
| 379 | |
| 380 | # Stack to create [future_steps, 29] then flatten to [future_steps * 29] |
| 381 | joint_pos_future = np.stack(future_joint_pos, axis=0).flatten() # [29 * 5] |
| 382 | joint_vel_future = np.stack(future_joint_vel, axis=0).flatten() # [29 * 5] |
| 383 | # breakpoint() |
| 384 | |
| 385 | cmd = np.concatenate([joint_pos_future, joint_vel_future], axis=0) |
| 386 | # breakpoint() |
| 387 | return cmd # [290] |
| 388 | |
| 389 | |
| 390 | def motion_anchor_pos_b_future(sim_data, motion_loader, t): |
no outgoing calls
no test coverage detected