| 68 | |
| 69 | |
| 70 | def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): |
| 71 | # Extract scene entities |
| 72 | robot: Articulation = scene["robot"] |
| 73 | # Define simulation stepping |
| 74 | sim_dt = sim.get_physics_dt() |
| 75 | |
| 76 | # Determine motion file path |
| 77 | if args_cli.motion: |
| 78 | # Use direct file path |
| 79 | motion_file = args_cli.motion |
| 80 | if not os.path.isfile(motion_file): |
| 81 | raise FileNotFoundError(f"Motion file not found: {motion_file}") |
| 82 | elif args_cli.registry_name: |
| 83 | # Download from wandb registry |
| 84 | try: |
| 85 | import wandb |
| 86 | except ImportError: |
| 87 | raise ImportError("wandb is required when using --registry_name. Install it with: pip install wandb") |
| 88 | |
| 89 | registry_name = args_cli.registry_name |
| 90 | if ":" not in registry_name: # Check if the registry name includes alias, if not, append ":latest" |
| 91 | registry_name += ":latest" |
| 92 | api = wandb.Api() |
| 93 | artifact = api.artifact(registry_name) |
| 94 | motion_file = str(pathlib.Path(artifact.download()) / "motion.npz") |
| 95 | else: |
| 96 | raise ValueError("Either --motion or --registry_name must be provided.") |
| 97 | |
| 98 | # Load npz file to get body names and determine body_indexes |
| 99 | # For K1, we typically use Trunk as anchor body (index 0) |
| 100 | # body_indexes should be a list of indices corresponding to the bodies we want to use |
| 101 | # For replay, we only need the anchor body (Trunk), which is typically at index 0 |
| 102 | body_indexes = [0] # Default to index 0 for anchor body (Trunk) |
| 103 | |
| 104 | motion = MotionLoader( |
| 105 | motion_file, |
| 106 | body_indexes, |
| 107 | tail_len=0, |
| 108 | device=str(sim.device), |
| 109 | ) |
| 110 | time_steps = torch.zeros(scene.num_envs, dtype=torch.long, device=sim.device) |
| 111 | |
| 112 | # Simulation loop |
| 113 | while simulation_app.is_running(): |
| 114 | time_steps += 1 |
| 115 | reset_ids = time_steps >= motion.time_step_total |
| 116 | time_steps[reset_ids] = 0 |
| 117 | |
| 118 | root_states = robot.data.default_root_state.clone() |
| 119 | root_states[:, :3] = motion.body_pos_w[time_steps][:, 0] + scene.env_origins[:, None, :] |
| 120 | root_states[:, 3:7] = motion.body_quat_w[time_steps][:, 0] |
| 121 | root_states[:, 7:10] = motion.body_lin_vel_w[time_steps][:, 0] |
| 122 | root_states[:, 10:] = motion.body_ang_vel_w[time_steps][:, 0] |
| 123 | |
| 124 | robot.write_root_state_to_sim(root_states) |
| 125 | robot.write_joint_state_to_sim(motion.joint_pos[time_steps], motion.joint_vel[time_steps]) |
| 126 | scene.write_data_to_sim() |
| 127 | sim.render() # We don't want physic (sim.step()) |