| 292 | |
| 293 | # ====== MotionLoader 类（参考 isaaclab） ====== |
| 294 | class MotionLoader: |
| 295 | def __init__(self, motion_file): |
| 296 | print(motion_file) |
| 297 | data = np.load(motion_file) |
| 298 | self.joint_pos = data["joint_pos"] # [T, 29] |
| 299 | self.joint_vel = data["joint_vel"] # [T, 29] |
| 300 | self.body_pos = data["body_pos_w"] # [T, N, 3] |
| 301 | self.body_ori = data["body_quat_w"] # [T, N, 4] |
| 302 | # self.body_ang_vel_w = data["body_ang_vel_w"] |
| 303 | self.fps = data["fps"] |
| 304 | self.T = self.joint_pos.shape[0] |
| 305 | |
| 306 | # self.body_pos[..., 2] += 0.015 |
| 307 | |
| 308 | # G1 body names from the config |
| 309 | self.body_names = [ |
| 310 | "pelvis", |
| 311 | "left_hip_roll_link", |
| 312 | "left_knee_link", |
| 313 | "left_ankle_roll_link", |
| 314 | "right_hip_roll_link", |
| 315 | "right_knee_link", |
| 316 | "right_ankle_roll_link", |
| 317 | "torso_link", |
| 318 | "left_shoulder_roll_link", |
| 319 | "left_elbow_link", |
| 320 | "left_wrist_yaw_link", |
| 321 | "right_shoulder_roll_link", |
| 322 | "right_elbow_link", |
| 323 | "right_wrist_yaw_link", |
| 324 | ] |
| 325 | self.anchor_body_name = anchor_body.name.lower() |
| 326 | self.anchor_body_index = anchor_body.value |
| 327 | |
| 328 | # Future steps configuration |
| 329 | self.future_steps = 5 |
| 330 | |
| 331 | |
| 332 | def quat_rotate_inverse_np(q: np.ndarray, v: np.ndarray) -> np.ndarray: |