Skip to content

Feat/13338 space satellite pnp cal #769

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

Draft
danwahl wants to merge 4 commits into
base: main
Choose a base branch
from
Draft

Feat/13338 space satellite pnp cal #769

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
ecbb4aa
feat(space_satellite_sim_camera_cal): use CalibrateCameraPosePnp for …
danwahl Jul 9, 2026
5a02e80
fix(space_satellite_sim_camera_cal): correct calibration tag frames f…
danwahl Jul 9, 2026
6f85559
feat(space_satellite_sim_camera_cal): add wrist-camera (eye-in-hand) …
danwahl Jul 9, 2026
6565293
refactor(space_satellite_sim_camera_cal): flat front-face wrist-cal t…
danwahl Jul 9, 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
4 changes: 2 additions & 2 deletions ...igs/space_satellite_sim_camera_cal/description/mujoco/assets/tag36_11_00003.png
View file
Open in desktop
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
3 changes: 3 additions & 0 deletions ...igs/space_satellite_sim_camera_cal/description/mujoco/assets/tag36_11_00004.png
View file
Open in desktop
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
3 changes: 3 additions & 0 deletions ...igs/space_satellite_sim_camera_cal/description/mujoco/assets/tag36_11_00005.png
View file
Open in desktop
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
4 changes: 4 additions & 0 deletions ...pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/gen3_7dof_assets.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -204,10 +204,14 @@
<texture type="2d" name="ar_marker_1" file="assets/tag36_11_00001.png" />
<texture type="2d" name="ar_marker_2" file="assets/tag36_11_00002.png" />
<texture type="2d" name="ar_marker_3" file="assets/tag36_11_00003.png" />
<texture type="2d" name="ar_marker_4" file="assets/tag36_11_00004.png" />
<texture type="2d" name="ar_marker_5" file="assets/tag36_11_00005.png" />
<material name="ar_marker_0" texture="ar_marker_0" texuniform="false" />
<material name="ar_marker_1" texture="ar_marker_1" texuniform="false" />
<material name="ar_marker_2" texture="ar_marker_2" texuniform="false" />
<material name="ar_marker_3" texture="ar_marker_3" texuniform="false" />
<material name="ar_marker_4" texture="ar_marker_4" texuniform="false" />
<material name="ar_marker_5" texture="ar_marker_5" texuniform="false" />
</asset>

<contact>
Expand Down
45 changes: 45 additions & 0 deletions src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/scene.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -182,6 +182,51 @@
<include file="gen3_7dof_body.xml" />
</body>
</body>

<!--
Satellite calibration tags for the WRIST camera (eye-in-hand). Three tag36h11
markers (ids 3,4,5) mounted flat and coplanar on the servicer's front (+x) gold
face, facing out into the arm's workspace (quat = Ry90 -> tag normal +x). They sit
on the arm-base side of the face, clear of the central docking mechanism. World-
fixed; matching URDF frames sat_tag_3/4/5 are children of `world`. Distinct ids
from the tool plate (0,1,2) so the wrist can see the tool without corrupting the
satellite-tag detections.
-->
<body name="satellite_tags">
<geom
name="sat_tag_3"
type="box"
quat="0.7071068 0 0.7071068 0"
size="0.031875 0.031875 0.0002"
pos="0.50 0.45 0.04"
rgba="1 1 1 1"
material="ar_marker_3"
contype="0"
conaffinity="0"
/>
<geom
name="sat_tag_4"
type="box"
quat="0.7071068 0 0.7071068 0"
size="0.031875 0.031875 0.0002"
pos="0.50 0.27 0.04"
rgba="1 1 1 1"
material="ar_marker_4"
contype="0"
conaffinity="0"
/>
<geom
name="sat_tag_5"
type="box"
quat="0.7071068 0 0.7071068 0"
size="0.031875 0.031875 0.0002"
pos="0.50 0.36 -0.12"
rgba="1 1 1 1"
material="ar_marker_5"
contype="0"
conaffinity="0"
/>
</body>
</worldbody>

<actuator>
Expand Down
91 changes: 57 additions & 34 deletions ...t_pro_kinova_configs/space_satellite_sim_camera_cal/description/space_satellite_sim.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -559,45 +559,32 @@
<child link="calibration_tool_link"/>
<origin xyz="0 0 0.169" rpy="0 0 0"/>
</joint>
<link name="calibration_tag_right">
<visual>
<origin xyz="0 0 0" rpy="0 1.5708 0"/>
<geometry>
<mesh filename="package://picknik_accessories/descriptions/ar_markers/Marker0.dae" scale=".05 .102 .102"/>
<!-- .dae tag side len = 0.5m, and tag size is 0.051m -> scale=0.102m -->
</geometry>
</visual>
</link>
<joint name="calibration_tag_right_joint" type="fixed">
<!--
AprilTag TF frames consumed by CalibrateCameraPosePnp. Frame calibration_tag_<id> is the
pose of the physical tag with detected id <id>: origin at the tag center, +Z out of the tag
face. These reproduce the tags rendered in simulation (description/mujoco/gen3_7dof_body.xml)
so the frames stay coincident with what the camera sees. The tag-center positions are the
same values the original center-based CalibrateCameraPose used (proven correct in sim); the
rpy flips Z so +Z points out of the tag face, which the corner-based PnP solve needs. Detected
ids follow the MuJoCo materials: 0 = tool left, 1 = tool right, 2 = bottom.
-->
<link name="calibration_tag_0"/>
<joint name="calibration_tag_0_joint" type="fixed">
<parent link="calibration_tool_link"/>
<child link="calibration_tag_right"/>
<origin xyz="-0.08625 -0.04925 -0.01" rpy="0 0 0"/>
<child link="calibration_tag_0"/>
<origin xyz="0.08625 -0.04925 -0.01" rpy="3.14159 0 0"/>
</joint>
<link name="calibration_tag_left">
<visual>
<origin xyz="0 0 0" rpy="0 1.5708 0"/>
<geometry>
<mesh filename="package://picknik_accessories/descriptions/ar_markers/Marker1.dae" scale=".05 .102 .102"/>
</geometry>
</visual>
</link>
<joint name="calibration_tag_left_joint" type="fixed">
<link name="calibration_tag_1"/>
<joint name="calibration_tag_1_joint" type="fixed">
<parent link="calibration_tool_link"/>
<child link="calibration_tag_left"/>
<origin xyz="0.08625 -0.04925 -0.01" rpy="0 0 0"/>
<child link="calibration_tag_1"/>
<origin xyz="-0.08625 -0.04925 -0.01" rpy="3.14159 0 0"/>
</joint>
<link name="calibration_tag_bottom">
<visual>
<origin xyz="0 0 0" rpy="0 1.5708 0"/>
<geometry>
<mesh filename="package://picknik_accessories/descriptions/ar_markers/Marker2.dae" scale=".05 .102 .102"/>
</geometry>
</visual>
</link>
<joint name="calibration_tag_bottom_joint" type="fixed">
<link name="calibration_tag_2"/>
<joint name="calibration_tag_2_joint" type="fixed">
<parent link="calibration_tool_link"/>
<child link="calibration_tag_bottom"/>
<origin xyz="0.0 -0.18125 -0.01" rpy="0 0 0"/>
<child link="calibration_tag_2"/>
<origin xyz="0.0 -0.18125 -0.01" rpy="3.14159 0 0"/>
</joint>

<link name="calibration_tool_frame"/>
Expand Down Expand Up @@ -631,6 +618,42 @@
<origin xyz="0 0 0" rpy="0 0 0" />
</xacro:realsense_d415>

<!-- Wrist Camera mount (eye-in-hand calibration). The nominal flange-to-camera pose
that the "Calibrate Wrist Camera" objective solves for; paste its result here to
correct the extrinsics. For the sim, mount == optical: bracelet_link -> optical is
Ry(180 deg) at [0, -0.056, -0.058] (MuJoCo renders /wrist_camera/* from this). -->
<link name="wrist_camera_mount_link" />
<joint name="wrist_camera_mount_joint" type="fixed">
<parent link="bracelet_link" />
<child link="wrist_camera_mount_link" />
<origin xyz="0 -0.056 -0.058" rpy="0 3.14159265358979 0" />
</joint>

<!-- Satellite calibration tags (static, world-fixed) for the wrist camera. tag36h11
ids 3/4/5, mounted flat and coplanar on the servicer's front (+x) gold face, on
the arm-base side of the central docking mechanism. rpy (Ry 90 deg) points +Z out
of the tag face (normal +x) for the corner-based PnP solve. Poses match the MuJoCo
satellite_tags geoms in description/mujoco/scene.xml. The "Calibrate Wrist Camera"
objective looks up frame 'sat_tag_<id>'. -->
<link name="sat_tag_3" />
<joint name="sat_tag_3_joint" type="fixed">
<parent link="world" />
<child link="sat_tag_3" />
<origin xyz="0.50 0.45 0.04" rpy="0 1.5708 0" />
</joint>
<link name="sat_tag_4" />
<joint name="sat_tag_4_joint" type="fixed">
<parent link="world" />
<child link="sat_tag_4" />
<origin xyz="0.50 0.27 0.04" rpy="0 1.5708 0" />
</joint>
<link name="sat_tag_5" />
<joint name="sat_tag_5_joint" type="fixed">
<parent link="world" />
<child link="sat_tag_5" />
<origin xyz="0.50 0.36 -0.12" rpy="0 1.5708 0" />
</joint>

<ros2_control name="kinova_mujoco_simulation" type="system">
<joint name="joint_1">
<command_interface name="position">
Expand Down
21 changes: 17 additions & 4 deletions ...nova_configs/space_satellite_sim_camera_cal/objectives/calibrate_camera_-_detect_tags.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,9 @@
_collapsed="true"
computed_poses_vector="{computed_poses_vector}"
camera_optical_frame="{camera_optical_frame}"
waypoint_name="Camera Calibration 1"
tag_frame_prefix="{tag_frame_prefix}"
mounting_pose_parent_frame="{mounting_pose_parent_frame}"
waypoint_name="{waypoint_1_name}"
camera_image_topic="{camera_image_topic}"
camera_info_topic="{camera_info_topic}"
/>
Expand All @@ -34,7 +36,9 @@
_collapsed="true"
computed_poses_vector="{computed_poses_vector}"
camera_optical_frame="{camera_optical_frame}"
waypoint_name="Camera Calibration 2"
tag_frame_prefix="{tag_frame_prefix}"
mounting_pose_parent_frame="{mounting_pose_parent_frame}"
waypoint_name="{waypoint_2_name}"
camera_image_topic="{camera_image_topic}"
camera_info_topic="{camera_info_topic}"
/>
Expand All @@ -43,7 +47,9 @@
_collapsed="true"
computed_poses_vector="{computed_poses_vector}"
camera_optical_frame="{camera_optical_frame}"
waypoint_name="Camera Calibration 3"
tag_frame_prefix="{tag_frame_prefix}"
mounting_pose_parent_frame="{mounting_pose_parent_frame}"
waypoint_name="{waypoint_3_name}"
camera_image_topic="{camera_image_topic}"
camera_info_topic="{camera_info_topic}"
/>
Expand All @@ -52,7 +58,9 @@
_collapsed="true"
computed_poses_vector="{computed_poses_vector}"
camera_optical_frame="{camera_optical_frame}"
waypoint_name="Camera Calibration 4"
tag_frame_prefix="{tag_frame_prefix}"
mounting_pose_parent_frame="{mounting_pose_parent_frame}"
waypoint_name="{waypoint_4_name}"
camera_image_topic="{camera_image_topic}"
camera_info_topic="{camera_info_topic}"
/>
Expand Down Expand Up @@ -142,6 +150,11 @@
name="mounting_pose_save_file"
default="calibrated_scene_camera_mounting_pose.txt"
/>
<inout_port name="tag_frame_prefix" default="calibration_tag_" />
<inout_port name="waypoint_1_name" default="Camera Calibration 1" />
<inout_port name="waypoint_2_name" default="Camera Calibration 2" />
<inout_port name="waypoint_3_name" default="Camera Calibration 3" />
<inout_port name="waypoint_4_name" default="Camera Calibration 4" />
</SubTree>
</TreeNodesModel>
</root>
17 changes: 15 additions & 2 deletions ...nfigs/space_satellite_sim_camera_cal/objectives/calibrate_camera_at_waypoint_-_detect.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@
/>
<Action
ID="WaitForDuration"
delay_duration="2.5"
delay_duration="5.0"
name="Wait for arm to settle."
/>
<Action ID="ResetPoseStampedVector" vector="{poses_at_waypoint_vec}" />
Expand All @@ -39,6 +39,7 @@
ID="Camera Pose from Cal Tool - Detect Tags"
_collapsed="true"
camera_optical_frame="{camera_optical_frame}"
tag_frame_prefix="{tag_frame_prefix}"
computed_pose="{computed_pose}"
camera_image_topic="{camera_image_topic}"
camera_info_topic="{camera_info_topic}"
Expand Down Expand Up @@ -68,10 +69,17 @@
marker_size="0.05"
pose="{average_pose_stamped}"
/>
<Action
ID="TransformPoseFrame"
input_pose="{average_pose_stamped}"
output_pose="{average_pose_in_mount_parent}"
target_frame_id="{mounting_pose_parent_frame}"
name="Express per-waypoint optical pose in mount parent frame (rigid to camera; invariant across waypoints, so the cross-waypoint average is unbiased)"
/>
<Action
ID="PushBackVector"
input_vector="{computed_poses_vector}"
element="{average_pose_stamped}"
element="{average_pose_in_mount_parent}"
output_vector="{computed_poses_vector}"
/>
</Control>
Expand All @@ -88,6 +96,11 @@
name="camera_optical_frame"
default="{camera_optical_frame}"
/>
<inout_port name="tag_frame_prefix" default="calibration_tag_" />
<inout_port
name="mounting_pose_parent_frame"
default="{mounting_pose_parent_frame}"
/>
<inout_port
name="computed_poses_vector"
default="{computed_poses_vector}"
Expand Down
33 changes: 33 additions & 0 deletions ...t_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/calibrate_wrist_camera.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Calibrate Wrist Camera">
<!--//////////-->
<BehaviorTree
ID="Calibrate Wrist Camera"
_description="Calibrate the wrist-mounted camera (eye-in-hand) from multiple waypoints by observing the static satellite AprilTags with CalibrateCameraPosePnp. Same subtree/behavior as the scene-camera calibration; only the frames, topics, tag prefix, and waypoints differ."
_favorite="true"
>
<SubTree
ID="Calibrate Camera - Detect Tags"
_collapsed="true"
camera_mounting_frame="wrist_camera_mount_link"
camera_optical_frame="wrist_camera_optical_frame"
mounting_pose_save_file="calibrated_wrist_camera_mounting_pose.txt"
camera_image_topic="/wrist_camera/color"
camera_info_topic="/wrist_camera/camera_info"
mounting_pose_parent_frame="bracelet_link"
tag_frame_prefix="sat_tag_"
waypoint_1_name="Wrist Camera Calibration 1"
waypoint_2_name="Wrist Camera Calibration 2"
waypoint_3_name="Wrist Camera Calibration 3"
waypoint_4_name="Wrist Camera Calibration 4"
/>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Calibrate Wrist Camera">
<MetadataFields>
<Metadata runnable="true" />
<Metadata subcategory="Perception - Camera Calibration" />
</MetadataFields>
</SubTree>
</TreeNodesModel>
</root>
Loading
Loading