diff --git a/spot_wrapper/calibration/spot_in_hand_camera_calibration.py b/spot_wrapper/calibration/spot_in_hand_camera_calibration.py index 40cac8e..94facd6 100644 --- a/spot_wrapper/calibration/spot_in_hand_camera_calibration.py +++ b/spot_wrapper/calibration/spot_in_hand_camera_calibration.py @@ -20,6 +20,7 @@ from bosdyn.client.frame_helpers import ( BODY_FRAME_NAME, HAND_FRAME_NAME, + WR1_FRAME_NAME, get_a_tform_b, ) from bosdyn.client.gripper_camera_param import GripperCameraParamClient @@ -123,15 +124,15 @@ def extract_calibration_parameters(self, calibration_dict: dict, tag: str) -> di calibration["rgb_intrinsic"] = np.asarray(calibration_dict[tag]["intrinsic"][0]["camera_matrix"]).reshape( (3, 3) ) - depth_to_rgb_T = np.array(calibration_dict[tag]["extrinsic"][1][0]["T"]).reshape((3, 1)) - depth_to_rgb_R = np.array(calibration_dict[tag]["extrinsic"][1][0]["R"]).reshape((3, 3)) - calibration["depth_to_rgb"] = np.vstack( - (np.hstack((depth_to_rgb_R, depth_to_rgb_T)), np.array([0, 0, 0, 1])) + rgb_to_depth_T = np.array(calibration_dict[tag]["extrinsic"][1][0]["T"]).reshape((3, 1)) + rgb_to_depth_R = np.array(calibration_dict[tag]["extrinsic"][1][0]["R"]).reshape((3, 3)) + calibration["rgb_to_depth"] = np.vstack( + (np.hstack((rgb_to_depth_R, rgb_to_depth_T)), np.array([0, 0, 0, 1])) ) - depth_to_planning_T = np.array(calibration_dict[tag]["extrinsic"][1]["planning_frame"]["T"]).reshape((3, 1)) - depth_to_planning_R = np.array(calibration_dict[tag]["extrinsic"][1]["planning_frame"]["R"]).reshape((3, 3)) - calibration["depth_to_planning_frame"] = np.vstack( - (np.hstack((depth_to_planning_R, depth_to_planning_T)), np.array([0, 0, 0, 1])) + depth_to_hand_T = np.array(calibration_dict[tag]["extrinsic"][1]["planning_frame"]["T"]).reshape((3, 1)) + depth_to_hand_R = np.array(calibration_dict[tag]["extrinsic"][1]["planning_frame"]["R"]).reshape((3, 3)) + calibration["depth_to_hand"] = np.vstack( + (np.hstack((depth_to_hand_R, depth_to_hand_T)), np.array([0, 0, 0, 1])) ) except KeyError as e: raise ValueError(f"Error: Missing key in the calibration data: {e}") @@ -152,7 +153,6 @@ def write_calibration_to_robot(self, cal_dict: dict, tag: str = "default") -> No cal = self.extract_calibration_parameters(cal_dict, tag) print("Pre Setting Param--------------------------------------------") - print(f"Calibration Data being sent to robot: \n {cal}") def convert_pinhole_intrinsic_to_proto(intrinsic_matrix: np.ndarray) -> ImageSource.PinholeModel: """Converts a 3x3 intrinsic matrix to a PinholeModel protobuf.""" @@ -161,28 +161,48 @@ def convert_pinhole_intrinsic_to_proto(intrinsic_matrix: np.ndarray) -> ImageSou pinhole_model.CameraIntrinsics.principal_point = (intrinsic_matrix[0, 2], intrinsic_matrix[1, 2]) return pinhole_model + hand_t_wr1_pose = get_a_tform_b( + self.robot_state_client.get_robot_state().kinematic_state.transforms_snapshot, + HAND_FRAME_NAME, + WR1_FRAME_NAME, + ) + hand_t_planning = np.eye(4) + hand_t_planning[:-1, -1] = np.array( + [ + hand_t_wr1_pose.x, + hand_t_wr1_pose.y, + hand_t_wr1_pose.z, + ] + ).reshape((3,)) + hand_t_planning[:-1, :-1] = hand_t_wr1_pose.rot.to_matrix() + logger.info(f"\n{hand_t_planning=}\n") + depth_intrinsics = cal["depth_intrinsic"] rgb_intrinsics = cal["rgb_intrinsic"] - depth_to_rgb = cal["depth_to_rgb"] - depth_to_planning_frame = cal["depth_to_planning_frame"] - rgb_to_planning_frame = np.linalg.inv(depth_to_rgb) @ depth_to_planning_frame + rgb_t_depth = cal["rgb_to_depth"] + + depth_to_planning = cal["depth_to_hand"] @ hand_t_planning + rgb_to_planning = rgb_t_depth @ depth_to_planning + + planning_t_depth = np.linalg.inv(depth_to_planning) + planning_t_rgb = np.linalg.inv(rgb_to_planning) # Converting calibration data to protobuf format depth_intrinsics_proto = convert_pinhole_intrinsic_to_proto(depth_intrinsics) rgb_intrinsics_proto = convert_pinhole_intrinsic_to_proto(rgb_intrinsics) - depth_to_planning_frame_proto = SE3Pose.from_matrix(depth_to_planning_frame).to_proto() - rgb_to_planning_frame_proto = SE3Pose.from_matrix(rgb_to_planning_frame).to_proto() + planning_t_depth_frame_proto = SE3Pose.from_matrix(planning_t_depth).to_proto() + planning_t_rgb_frame_proto = SE3Pose.from_matrix(planning_t_rgb).to_proto() set_req = gripper_camera_param_pb2.SetGripperCameraCalibrationRequest( gripper_cam_cal=gripper_camera_param_pb2.GripperCameraCalibrationProto( depth=gripper_camera_param_pb2.GripperDepthCameraCalibrationParams( - wr1_tform_sensor=depth_to_planning_frame_proto, + wr1_tform_sensor=planning_t_depth_frame_proto, intrinsics=gripper_camera_param_pb2.GripperDepthCameraCalibrationParams.DepthCameraIntrinsics( pinhole=depth_intrinsics_proto ), ), color=gripper_camera_param_pb2.GripperColorCameraCalibrationParams( - wr1_tform_sensor=rgb_to_planning_frame_proto, + wr1_tform_sensor=planning_t_rgb_frame_proto, intrinsics=[ gripper_camera_param_pb2.GripperColorCameraCalibrationParams.ColorCameraIntrinsics( pinhole=rgb_intrinsics_proto @@ -191,7 +211,6 @@ def convert_pinhole_intrinsic_to_proto(intrinsic_matrix: np.ndarray) -> ImageSou ), ) ) - # Send the request to the robot try: result = self.gripper_camera_client.set_camera_calib(set_req) @@ -262,6 +281,24 @@ def adjust_standing_height(height: float) -> None: old_pose, ready_pose = self.offset_cameras_from_current_view(transform_offset=transform_offset) return ready_pose + def grab_state_as_transform(self) -> np.ndarray: + robot_state = self.robot_state_client.get_robot_state() + origin_t_planning_frame = get_a_tform_b( + robot_state.kinematic_state.transforms_snapshot, + BODY_FRAME_NAME, + HAND_FRAME_NAME, + ) + pose_transform = np.eye(4) + pose_transform[:-1, -1] = np.array( + [ + origin_t_planning_frame.x, + origin_t_planning_frame.y, + origin_t_planning_frame.z, + ] + ).reshape((3,)) + pose_transform[:-1, :-1] = origin_t_planning_frame.rot.to_matrix() + return pose_transform + def offset_cameras_from_current_view( self, transform_offset: np.ndarray, @@ -269,26 +306,8 @@ def offset_cameras_from_current_view( use_body: bool = False, duration_sec: float = 1.0, ) -> Tuple[np.ndarray, np.ndarray]: - def grab_state_as_transform() -> np.ndarray: - robot_state = self.robot_state_client.get_robot_state() - origin_t_planning_frame = get_a_tform_b( - robot_state.kinematic_state.transforms_snapshot, - BODY_FRAME_NAME, - HAND_FRAME_NAME, - ) - pose_transform = np.eye(4) - pose_transform[:-1, -1] = np.array( - [ - origin_t_planning_frame.x, - origin_t_planning_frame.y, - origin_t_planning_frame.z, - ] - ).reshape((3,)) - pose_transform[:-1, :-1] = origin_t_planning_frame.rot.to_matrix() - return pose_transform - if origin_t_planning_frame is None: - initial_pose = grab_state_as_transform() + initial_pose = self.grab_state_as_transform() else: initial_pose = origin_t_planning_frame @@ -328,7 +347,7 @@ def grab_state_as_transform() -> np.ndarray: timeout_sec=duration_sec * 2, ) sleep(duration_sec * 0.5) # settle before grabbing new state for better quality - return (initial_pose, grab_state_as_transform()) # second value is new value + return (initial_pose, self.grab_state_as_transform()) # second value is new value def shutdown(self) -> None: stow_arm_command = RobotCommandBuilder.arm_stow_command()