Run a movement test with the given configuration.
(test_setup, test_config, test_cfg, aux_function=None)
| 216 | |
| 217 | |
| 218 | def run_movement_test(test_setup, test_config, test_cfg, aux_function=None): |
| 219 | """Run a movement test with the given configuration.""" |
| 220 | env = test_setup["env"] |
| 221 | num_joints_in_robot_hands = test_setup["num_joints_in_robot_hands"] |
| 222 | |
| 223 | left_hand_poses = np.array(test_config["left_hand_pose"], dtype=np.float32) |
| 224 | right_hand_poses = np.array(test_config["right_hand_pose"], dtype=np.float32) |
| 225 | |
| 226 | curr_pose_idx = 0 |
| 227 | test_counter = 0 |
| 228 | num_runs = 0 |
| 229 | |
| 230 | with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode(): |
| 231 | obs, _ = env.reset() |
| 232 | |
| 233 | # Make the first phase longer than subsequent ones |
| 234 | initial_steps = test_cfg["allowed_steps_to_settle"] |
| 235 | phase = "initial" |
| 236 | steps_in_phase = 0 |
| 237 | |
| 238 | while simulation_app.is_running() and not simulation_app.is_exiting(): |
| 239 | num_runs += 1 |
| 240 | steps_in_phase += 1 |
| 241 | |
| 242 | # Call auxiliary function if provided |
| 243 | if aux_function is not None: |
| 244 | aux_function(num_runs) |
| 245 | |
| 246 | # Create actions from hand poses and joint positions |
| 247 | setpoint_poses = np.concatenate([left_hand_poses[curr_pose_idx], right_hand_poses[curr_pose_idx]]) |
| 248 | actions = np.concatenate([setpoint_poses, np.zeros(num_joints_in_robot_hands)]) |
| 249 | actions = torch.tensor(actions, device=env.device, dtype=torch.float32) |
| 250 | # Append base command for Locomanipulation environments with fixed height |
| 251 | if test_setup["env_cfg"].__class__.__name__ == "LocomanipulationG1EnvCfg": |
| 252 | # Use a named variable for base height for clarity and maintainability |
| 253 | BASE_HEIGHT = 0.72 |
| 254 | base_command = torch.zeros(4, device=env.device, dtype=actions.dtype) |
| 255 | base_command[3] = BASE_HEIGHT |
| 256 | actions = torch.cat([actions, base_command]) |
| 257 | actions = actions.repeat(env.num_envs, 1) |
| 258 | |
| 259 | # Step environment |
| 260 | obs, _, _, _, _ = env.step(actions) |
| 261 | |
| 262 | # Determine the step interval for error checking |
| 263 | if phase == "initial": |
| 264 | check_interval = initial_steps |
| 265 | else: |
| 266 | check_interval = test_config["allowed_steps_per_motion"] |
| 267 | |
| 268 | # Check convergence and verify errors |
| 269 | if steps_in_phase % check_interval == 0: |
| 270 | print("Computing errors...") |
| 271 | errors = compute_errors( |
| 272 | test_setup, |
| 273 | env, |
| 274 | left_hand_poses[curr_pose_idx], |
| 275 | right_hand_poses[curr_pose_idx], |
no test coverage detected