Skip to content

[SW-4178] Spot calibration params bug #189

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
tcappellari-bdai merged 5 commits into from
Jan 22, 2026
Merged

[SW-4178] Spot calibration params bug #189

Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
ec97b17
maybe fixed??
tcappellari-bdai Dec 19, 2025
9433177
some changes and debugging things happened
tcappellari-bdai Jan 5, 2026
19da2c4
frame transforms look correct
tcappellari-bdai Jan 8, 2026
314b90b
pre-commit
tcappellari-bdai Jan 9, 2026
538c5e2
Merge branch 'main' into tcapp/calibration_bug
tcappellari-bdai Jan 22, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
93 changes: 56 additions & 37 deletions spot_wrapper/calibration/spot_in_hand_camera_calibration.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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}")
Expand All @@ -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."""
Expand All @@ -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
Expand All @@ -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)
Expand Down Expand Up @@ -262,33 +281,33 @@ 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,
origin_t_planning_frame: Optional[np.ndarray] = None,
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

Expand Down Expand Up @@ -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()
Expand Down