Verify that all error metrics are within tolerance.
(errors, test_setup, tolerances)
| 389 | |
| 390 | |
| 391 | def verify_errors(errors, test_setup, tolerances): |
| 392 | """Verify that all error metrics are within tolerance.""" |
| 393 | env = test_setup["env"] |
| 394 | device = env.device |
| 395 | num_envs = env.num_envs |
| 396 | zero_tensor = torch.zeros(num_envs, device=device) |
| 397 | |
| 398 | for hand in ["left", "right"]: |
| 399 | # Check PD controller errors |
| 400 | pd_error_norm = torch.norm(errors[f"{hand}_pd_error"], dim=1) |
| 401 | torch.testing.assert_close( |
| 402 | pd_error_norm, |
| 403 | zero_tensor, |
| 404 | rtol=0.0, |
| 405 | atol=tolerances["pd_position"], |
| 406 | msg=( |
| 407 | f"{hand.capitalize()} hand PD controller error ({pd_error_norm.item():.6f}) exceeds tolerance" |
| 408 | f" ({tolerances['pd_position']:.6f})" |
| 409 | ), |
| 410 | ) |
| 411 | |
| 412 | # Check IK position errors |
| 413 | pos_error_norm = torch.norm(errors[f"{hand}_pos_error"], dim=1) |
| 414 | torch.testing.assert_close( |
| 415 | pos_error_norm, |
| 416 | zero_tensor, |
| 417 | rtol=0.0, |
| 418 | atol=tolerances["position"], |
| 419 | msg=( |
| 420 | f"{hand.capitalize()} hand IK position error ({pos_error_norm.item():.6f}) exceeds tolerance" |
| 421 | f" ({tolerances['position']:.6f})" |
| 422 | ), |
| 423 | ) |
| 424 | |
| 425 | # Check rotation errors |
| 426 | rot_error_max = torch.max(errors[f"{hand}_rot_error"]) |
| 427 | torch.testing.assert_close( |
| 428 | rot_error_max, |
| 429 | torch.zeros_like(rot_error_max), |
| 430 | rtol=0.0, |
| 431 | atol=tolerances["rotation"], |
| 432 | msg=( |
| 433 | f"{hand.capitalize()} hand IK rotation error ({rot_error_max.item():.6f}) exceeds tolerance" |
| 434 | f" ({tolerances['rotation']:.6f})" |
| 435 | ), |
| 436 | ) |
| 437 | |
| 438 | |
| 439 | def print_debug_info(errors, test_counter): |