Reset the simulation to a random initial state. This function: 1. Resets robot to home configuration 2. Randomizes target position and orientation 3. Randomizes block position and orientation 4. Waits for user's hand to be tracked 5. Smoothly interpolates robot to u
(model, data, controller, streamer)
| 242 | |
| 243 | |
| 244 | def reset_simulation(model, data, controller, streamer): |
| 245 | """ |
| 246 | Reset the simulation to a random initial state. |
| 247 | |
| 248 | This function: |
| 249 | 1. Resets robot to home configuration |
| 250 | 2. Randomizes target position and orientation |
| 251 | 3. Randomizes block position and orientation |
| 252 | 4. Waits for user's hand to be tracked |
| 253 | 5. Smoothly interpolates robot to user's hand position |
| 254 | |
| 255 | Args: |
| 256 | model: MuJoCo model |
| 257 | data: MuJoCo data |
| 258 | controller: OperationalSpaceController instance |
| 259 | streamer: VisionProStreamer instance |
| 260 | """ |
| 261 | key_name = "home" |
| 262 | key_id = model.key(key_name).id |
| 263 | mujoco.mj_resetDataKeyframe(model, data, key_id) |
| 264 | |
| 265 | mocap_target_id = model.body("target_pos").mocapid[0] |
| 266 | |
| 267 | # Random 90-degree rotation for target |
| 268 | angles = [np.deg2rad(np.random.choice([-90, 0, 90])) for _ in range(3)] |
| 269 | rot = R.from_euler('xyz', angles).as_quat() |
| 270 | |
| 271 | data.mocap_pos[mocap_target_id][0:2] = np.array([0.5, 0.1]) + np.random.uniform(-0.1, 0.1, 2) |
| 272 | data.mocap_pos[mocap_target_id][2] = 0.05 |
| 273 | data.mocap_quat[mocap_target_id] = rot |
| 274 | |
| 275 | # Randomize block position |
| 276 | data.qpos[9:11] = np.array([0.5, -0.1]) + np.random.uniform(-0.1, 0.1, 2) |
| 277 | data.qpos[11] = 0.05 |
| 278 | |
| 279 | # Reset mocap target |
| 280 | data.mocap_pos[controller.mocap_id] = np.array([0.5, 0.0, 0.5]) |
| 281 | data.mocap_quat[controller.mocap_id] = np.array([0, 1, 0, 0]) |
| 282 | |
| 283 | data.qvel[:] = 0.0 |
| 284 | |
| 285 | # Random rotation for block |
| 286 | angles = [np.deg2rad(np.random.choice([-90, 0, 90])) for _ in range(3)] |
| 287 | rot = R.from_euler('xyz', angles).as_quat() |
| 288 | data.qpos[12:16] = rot |
| 289 | |
| 290 | data.qacc_warmstart[:] = 0.0 |
| 291 | mujoco.mj_forward(model, data) |
| 292 | |
| 293 | print("🖐️ Move your hand to start position...") |
| 294 | |
| 295 | # Wait for hand tracking and follow hand |
| 296 | for i in range(1000): |
| 297 | hand = streamer.get_latest() |
| 298 | if hand is None: |
| 299 | time.sleep(1 / 60.0) |
| 300 | continue |
| 301 |
no test coverage detected