Compute all error metrics for the current state.
(
test_setup, env, left_target_pose, right_target_pose, left_eef_urdf_link_name, right_eef_urdf_link_name
)
| 319 | |
| 320 | |
| 321 | def compute_errors( |
| 322 | test_setup, env, left_target_pose, right_target_pose, left_eef_urdf_link_name, right_eef_urdf_link_name |
| 323 | ): |
| 324 | """Compute all error metrics for the current state.""" |
| 325 | action_term = test_setup["action_term"] |
| 326 | pink_controllers = test_setup["pink_controllers"] |
| 327 | articulation = test_setup["articulation"] |
| 328 | test_kinematics_model = test_setup["test_kinematics_model"] |
| 329 | left_target_link_name = test_setup["left_target_link_name"] |
| 330 | right_target_link_name = test_setup["right_target_link_name"] |
| 331 | |
| 332 | # Get current hand positions and orientations |
| 333 | left_hand_pos, left_hand_rot = get_link_pose(env, left_target_link_name) |
| 334 | right_hand_pos, right_hand_rot = get_link_pose(env, right_target_link_name) |
| 335 | |
| 336 | # Create setpoint tensors |
| 337 | device = env.device |
| 338 | num_envs = env.num_envs |
| 339 | left_hand_pose_setpoint = torch.tensor(left_target_pose, device=device).unsqueeze(0).repeat(num_envs, 1) |
| 340 | right_hand_pose_setpoint = torch.tensor(right_target_pose, device=device).unsqueeze(0).repeat(num_envs, 1) |
| 341 | |
| 342 | # Calculate position and rotation errors |
| 343 | left_pos_error = left_hand_pose_setpoint[:, :3] - left_hand_pos |
| 344 | right_pos_error = right_hand_pose_setpoint[:, :3] - right_hand_pos |
| 345 | left_rot_error = calculate_rotation_error(left_hand_rot, left_hand_pose_setpoint[:, 3:]) |
| 346 | right_rot_error = calculate_rotation_error(right_hand_rot, right_hand_pose_setpoint[:, 3:]) |
| 347 | |
| 348 | # Calculate PD controller errors |
| 349 | ik_controller = pink_controllers[0] |
| 350 | isaaclab_controlled_joint_ids = action_term._isaaclab_controlled_joint_ids |
| 351 | |
| 352 | # Get current and target positions for controlled joints only |
| 353 | curr_joints = articulation.data.joint_pos[:, isaaclab_controlled_joint_ids].cpu().numpy()[0] |
| 354 | target_joints = action_term.processed_actions[:, : len(isaaclab_controlled_joint_ids)].cpu().numpy()[0] |
| 355 | |
| 356 | # Reorder joints for Pink IK (using controlled joint ordering) |
| 357 | curr_joints = np.array(curr_joints)[ik_controller.isaac_lab_to_pink_controlled_ordering] |
| 358 | target_joints = np.array(target_joints)[ik_controller.isaac_lab_to_pink_controlled_ordering] |
| 359 | |
| 360 | # Run forward kinematics |
| 361 | test_kinematics_model.update(curr_joints) |
| 362 | left_curr_pos = test_kinematics_model.get_transform_frame_to_world(frame=left_eef_urdf_link_name).translation |
| 363 | right_curr_pos = test_kinematics_model.get_transform_frame_to_world(frame=right_eef_urdf_link_name).translation |
| 364 | |
| 365 | test_kinematics_model.update(target_joints) |
| 366 | left_target_pos = test_kinematics_model.get_transform_frame_to_world(frame=left_eef_urdf_link_name).translation |
| 367 | right_target_pos = test_kinematics_model.get_transform_frame_to_world(frame=right_eef_urdf_link_name).translation |
| 368 | |
| 369 | # Calculate PD errors |
| 370 | left_pd_error = ( |
| 371 | torch.tensor(left_target_pos - left_curr_pos, device=device, dtype=torch.float32) |
| 372 | .unsqueeze(0) |
| 373 | .repeat(num_envs, 1) |
| 374 | ) |
| 375 | right_pd_error = ( |
| 376 | torch.tensor(right_target_pos - right_curr_pos, device=device, dtype=torch.float32) |
| 377 | .unsqueeze(0) |
| 378 | .repeat(num_envs, 1) |
no test coverage detected