From ecbb4aab04855c1e78b7b4146ad9eb3a6e27c8f5 Mon Sep 17 00:00:00 2001
From: Dan Wahl <drwahl@gmail.com>
Date: Wed, 8 Jul 2026 23:05:32 -0500
Subject: [PATCH 1/4] feat(space_satellite_sim_camera_cal): use
 CalibrateCameraPosePnp for scene camera cal

Rewire the scene-camera calibration leaf to GetImage + GetCameraInfo +
CalibrateCameraPosePnp (single joint PnP over all tag corners), replacing the
DetectAprilTags + per-tag GetDetectionPose + CalibrateCameraPose path.

Replace the decorative calibration_tag_right/left/bottom links with TF-only
calibration_tag_0/1/2 frames that reproduce the MuJoCo-rendered AprilTag poses
exactly (via a calibration_tags_root matching MuJoCo's calibration_tool body),
so the behavior's per-id TF lookups resolve to what the camera actually sees.
---
 .../description/space_satellite_sim.xacro     | 65 +++++++++----------
 ...amera_pose_from_cal_tool_-_detect_tags.xml | 47 ++------------
 2 files changed, 36 insertions(+), 76 deletions(-)

diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/space_satellite_sim.xacro b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/space_satellite_sim.xacro
index c306f2668..f24c7d8c1 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/space_satellite_sim.xacro
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/space_satellite_sim.xacro
@@ -559,45 +559,38 @@
     <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 and
+    +Z out of the tag face. These reproduce the tags rendered in simulation (see
+    description/mujoco/gen3_7dof_body.xml) so the frames stay coincident with what the
+    camera sees: the MuJoCo "calibration_tool" body applies a -90 deg yaw that
+    calibration_tool_link does not, so the tags hang off a matching intermediate frame.
+    Detected ids follow the MuJoCo materials: 0 = tool left, 1 = tool right, 2 = bottom.
+  -->
+  <link name="calibration_tags_root"/>
+  <joint name="calibration_tags_root_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_tags_root"/>
+    <origin xyz="0 0 0" rpy="0 0 -1.5708"/>
   </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">
-    <parent link="calibration_tool_link"/>
-    <child link="calibration_tag_left"/>
-    <origin xyz="0.08625 -0.04925 -0.01" rpy="0 0 0"/>
+  <link name="calibration_tag_0"/>
+  <joint name="calibration_tag_0_joint" type="fixed">
+    <parent link="calibration_tags_root"/>
+    <child link="calibration_tag_0"/>
+    <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">
-    <parent link="calibration_tool_link"/>
-    <child link="calibration_tag_bottom"/>
-    <origin xyz="0.0 -0.18125 -0.01" rpy="0 0 0"/>
+  <link name="calibration_tag_1"/>
+  <joint name="calibration_tag_1_joint" type="fixed">
+    <parent link="calibration_tags_root"/>
+    <child link="calibration_tag_1"/>
+    <origin xyz="-0.08625 -0.04925 -0.01" rpy="3.14159 0 0"/>
+  </joint>
+  <link name="calibration_tag_2"/>
+  <joint name="calibration_tag_2_joint" type="fixed">
+    <parent link="calibration_tags_root"/>
+    <child link="calibration_tag_2"/>
+    <origin xyz="0.0 -0.18125 -0.01" rpy="3.14159 0 0"/>
   </joint>
 
   <link name="calibration_tool_frame"/>
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/camera_pose_from_cal_tool_-_detect_tags.xml b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/camera_pose_from_cal_tool_-_detect_tags.xml
index 56f8e7b7d..a5e9b731b 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/camera_pose_from_cal_tool_-_detect_tags.xml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/camera_pose_from_cal_tool_-_detect_tags.xml
@@ -7,7 +7,7 @@
   <BehaviorTree
     ID="Camera Pose from Cal Tool - Detect Tags"
     computed_pose="{computed_pose}"
-    _description="Directly detect the calibration tool's AprilTags to compute the camera pose."
+    _description="Compute the camera optical pose from the calibration tool's AprilTags with a single joint PnP solve over every tag corner."
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <Action
@@ -23,48 +23,15 @@
         topic_name="{camera_info_topic}"
       />
       <Action
-        ID="DetectAprilTags"
-        camera_info="{camera_info}"
-        detections="{detections}"
+        ID="CalibrateCameraPosePnp"
         image="{image}"
-        apriltag_family_name="36h11"
-        max_hamming="0"
-        n_threads="1"
-        quad_decimate="2.0"
-        quad_sigma="0.0"
-        refine_edges="true"
-        tag_size="0.051"
-      />
-      <Action
-        ID="GetDetectionPose"
-        detections="{detections}"
-        name="topRight"
-        target_id="1"
-        detection_pose="{right_pose}"
-      />
-      <Action
-        ID="GetDetectionPose"
-        detections="{detections}"
-        target_id="0"
-        name="topLeft"
-        detection_pose="{left_pose}"
-      />
-      <Action
-        ID="GetDetectionPose"
-        detections="{detections}"
-        target_id="2"
-        name="bottom"
-        detection_pose="{bottom_pose}"
-      />
-      <Action
-        ID="CalibrateCameraPose"
+        camera_info="{camera_info}"
         base_link="base_link"
-        calibration_tool_frame="calibration_tool_frame"
+        tag_frame_prefix="calibration_tag_"
+        tag_size="0.051"
+        tag_family="tag36h11"
+        min_tags="3"
         computed_pose="{computed_pose}"
-        right_tag_pose="{right_pose}"
-        left_tag_pose="{left_pose}"
-        bottom_tag_pose="{bottom_pose}"
-        camera_optical_frame="{camera_optical_frame}"
       />
       <Action
         ID="VisualizePose"

From 5a02e80c4bc3c6ac527966abff933196cd3a56b2 Mon Sep 17 00:00:00 2001
From: Dan Wahl <drwahl@gmail.com>
Date: Thu, 9 Jul 2026 01:35:25 -0500
Subject: [PATCH 2/4] fix(space_satellite_sim_camera_cal): correct calibration
 tag frames for PnP

Part B's tag frames added a spurious Rz(-90) parent rotation
(calibration_tags_root) that displaced the tag positions from where they
render in MuJoCo. CalibrateCameraPosePnp uses tag corners (orientation-
and position-sensitive), so this made it drift to a different phantom
camera pose at each waypoint, whereas the old center-only
CalibrateCameraPose tolerated it.

Parent calibration_tag_0/1/2 directly to calibration_tool_link at the
proven tag-center offsets (same values the original centers-based
calibration used); rpy="3.14159 0 0" keeps +Z out of the tag face for
the corner PnP solve. Verified in sim: recovered optical pose is now
consistent across all waypoints and the saved mount matches the URDF
scene camera mount within noise.
---
 .../description/space_satellite_sim.xacro     | 26 +++++++------------
 1 file changed, 10 insertions(+), 16 deletions(-)

diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/space_satellite_sim.xacro b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/space_satellite_sim.xacro
index f24c7d8c1..73e61661d 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/space_satellite_sim.xacro
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/space_satellite_sim.xacro
@@ -560,35 +560,29 @@
     <origin xyz="0 0 0.169" rpy="0 0 0"/>
   </joint>
   <!--
-    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 and
-    +Z out of the tag face. These reproduce the tags rendered in simulation (see
-    description/mujoco/gen3_7dof_body.xml) so the frames stay coincident with what the
-    camera sees: the MuJoCo "calibration_tool" body applies a -90 deg yaw that
-    calibration_tool_link does not, so the tags hang off a matching intermediate frame.
-    Detected ids follow the MuJoCo materials: 0 = tool left, 1 = tool right, 2 = bottom.
+    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_tags_root"/>
-  <joint name="calibration_tags_root_joint" type="fixed">
-    <parent link="calibration_tool_link"/>
-    <child link="calibration_tags_root"/>
-    <origin xyz="0 0 0" rpy="0 0 -1.5708"/>
-  </joint>
   <link name="calibration_tag_0"/>
   <joint name="calibration_tag_0_joint" type="fixed">
-    <parent link="calibration_tags_root"/>
+    <parent link="calibration_tool_link"/>
     <child link="calibration_tag_0"/>
     <origin xyz="0.08625 -0.04925 -0.01" rpy="3.14159 0 0"/>
   </joint>
   <link name="calibration_tag_1"/>
   <joint name="calibration_tag_1_joint" type="fixed">
-    <parent link="calibration_tags_root"/>
+    <parent link="calibration_tool_link"/>
     <child link="calibration_tag_1"/>
     <origin xyz="-0.08625 -0.04925 -0.01" rpy="3.14159 0 0"/>
   </joint>
   <link name="calibration_tag_2"/>
   <joint name="calibration_tag_2_joint" type="fixed">
-    <parent link="calibration_tags_root"/>
+    <parent link="calibration_tool_link"/>
     <child link="calibration_tag_2"/>
     <origin xyz="0.0 -0.18125 -0.01" rpy="3.14159 0 0"/>
   </joint>

From 6f85559a897cfb58afe0b3dd1a966a46c9c49c56 Mon Sep 17 00:00:00 2001
From: Dan Wahl <drwahl@gmail.com>
Date: Thu, 9 Jul 2026 10:48:28 -0500
Subject: [PATCH 3/4] feat(space_satellite_sim_camera_cal): add wrist-camera
 (eye-in-hand) PnP calibration

Add a "Calibrate Wrist Camera" objective that calibrates the wrist-mounted
camera against three static satellite AprilTags, reusing the same
CalibrateCameraPosePnp subtree as the scene-camera (eye-to-hand) calibration.
Only the frames, topics, tag prefix, and waypoints differ.

- Scene/URDF: three tag36h11 markers (ids 3/4/5, distinct from the tool plate
  0/1/2) as angled plates on the drum top, plus a wrist_camera_mount_link frame
  on the flange to receive the calibrated extrinsics.
- Objectives: thread tag_frame_prefix, mounting_pose_parent_frame, and
  waypoint_N_name through the shared subtrees (defaults preserve the scene-camera
  path unchanged); new thin calibrate_wrist_camera.xml wires the wrist ports.
- Per-waypoint fix: express each waypoint's optical pose in the mount-parent
  frame before averaging. For a moving (eye-in-hand) camera the optical pose in
  base_link differs per waypoint, so averaging it directly biased the result;
  the mount-parent frame is rigid to the camera, so the cross-waypoint average is
  unbiased. This is a no-op for the eye-to-hand case (parent == base_link).

Verified in sim: recovers the declared wrist mount [0, -0.056, -0.058] Ry180 to
sub-millimeter, with per-waypoint PnP reprojection RMS ~0.38 px.
---
 .../mujoco/assets/tag36_11_00003.png          |   4 +-
 .../mujoco/assets/tag36_11_00004.png          |   3 +
 .../mujoco/assets/tag36_11_00005.png          |   3 +
 .../description/mujoco/gen3_7dof_assets.xml   |   4 +
 .../description/mujoco/scene.xml              |  43 ++++
 .../description/space_satellite_sim.xacro     |  35 ++++
 .../calibrate_camera_-_detect_tags.xml        |  21 +-
 .../calibrate_camera_at_waypoint_-_detect.xml |  17 +-
 .../objectives/calibrate_wrist_camera.xml     |  33 ++++
 ...amera_pose_from_cal_tool_-_detect_tags.xml |   3 +-
 .../waypoints/waypoints.yaml                  | 184 ++++++++++++++++++
 11 files changed, 341 insertions(+), 9 deletions(-)
 create mode 100644 src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/assets/tag36_11_00004.png
 create mode 100644 src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/assets/tag36_11_00005.png
 create mode 100644 src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/calibrate_wrist_camera.xml

diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/assets/tag36_11_00003.png b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/assets/tag36_11_00003.png
index 994d7f42a..5fdf2ad30 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/assets/tag36_11_00003.png
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/assets/tag36_11_00003.png
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:6945a263eadff4ab15a426aad866c00dd9b8c756a10392d8b7e18c0dd0509443
-size 5480
+oid sha256:6c6ef757c14b3b961825df85f6cc196c1dea8bcc65ab4afbdb6d31b7855e20b7
+size 3259
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/assets/tag36_11_00004.png b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/assets/tag36_11_00004.png
new file mode 100644
index 000000000..a50d13f95
--- /dev/null
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/assets/tag36_11_00004.png
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a066e8a73f4c801c88f36409c95a6974ca61ecc4a20a755a79d4ada6a2ded355
+size 3265
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/assets/tag36_11_00005.png b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/assets/tag36_11_00005.png
new file mode 100644
index 000000000..8bcff332e
--- /dev/null
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/assets/tag36_11_00005.png
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:04c87a3eaf6310fc28b6470b9c2eee46373ac07c8abacee1a1d6e972d0dc8371
+size 3265
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/gen3_7dof_assets.xml b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/gen3_7dof_assets.xml
index 5ce229ec8..c9c313906 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/gen3_7dof_assets.xml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/gen3_7dof_assets.xml
@@ -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>
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/scene.xml b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/scene.xml
index ad9574167..c7d839190 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/scene.xml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/scene.xml
@@ -182,6 +182,49 @@
         <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) on the drum top, tilted ~30 deg about Y (quat = Ry30) so the
+      tag face angles toward the servicer arm. 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.9659258 0 0.2588190 0"
+        size="0.031875 0.031875 0.0002"
+        pos="0.42 0.46 0.274"
+        rgba="1 1 1 1"
+        material="ar_marker_3"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="sat_tag_4"
+        type="box"
+        quat="0.9659258 0 0.2588190 0"
+        size="0.031875 0.031875 0.0002"
+        pos="0.42 0.66 0.274"
+        rgba="1 1 1 1"
+        material="ar_marker_4"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="sat_tag_5"
+        type="box"
+        quat="0.9659258 0 0.2588190 0"
+        size="0.031875 0.031875 0.0002"
+        pos="0.20 0.56 0.274"
+        rgba="1 1 1 1"
+        material="ar_marker_5"
+        contype="0"
+        conaffinity="0"
+      />
+    </body>
   </worldbody>
 
   <actuator>
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/space_satellite_sim.xacro b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/space_satellite_sim.xacro
index 73e61661d..374daa4a6 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/space_satellite_sim.xacro
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/space_satellite_sim.xacro
@@ -618,6 +618,41 @@
     <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 as angled plates on the drum top facing the servicer arm; the
+       rpy tilt (Ry 30 deg) points +Z out of the tag face 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.42 0.46 0.274" rpy="0 0.5236 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.42 0.66 0.274" rpy="0 0.5236 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.20 0.56 0.274" rpy="0 0.5236 0" />
+  </joint>
+
   <ros2_control name="kinova_mujoco_simulation" type="system">
     <joint name="joint_1">
       <command_interface name="position">
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/calibrate_camera_-_detect_tags.xml b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/calibrate_camera_-_detect_tags.xml
index 2ebe3a0ec..fd08580a1 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/calibrate_camera_-_detect_tags.xml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/calibrate_camera_-_detect_tags.xml
@@ -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}"
       />
@@ -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}"
       />
@@ -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}"
       />
@@ -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}"
       />
@@ -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>
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/calibrate_camera_at_waypoint_-_detect.xml b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/calibrate_camera_at_waypoint_-_detect.xml
index 423381e0d..2385e33a8 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/calibrate_camera_at_waypoint_-_detect.xml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/calibrate_camera_at_waypoint_-_detect.xml
@@ -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}" />
@@ -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}"
@@ -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>
@@ -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}"
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/calibrate_wrist_camera.xml b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/calibrate_wrist_camera.xml
new file mode 100644
index 000000000..c884f3046
--- /dev/null
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/calibrate_wrist_camera.xml
@@ -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>
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/camera_pose_from_cal_tool_-_detect_tags.xml b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/camera_pose_from_cal_tool_-_detect_tags.xml
index a5e9b731b..a8607df38 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/camera_pose_from_cal_tool_-_detect_tags.xml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/objectives/camera_pose_from_cal_tool_-_detect_tags.xml
@@ -27,7 +27,7 @@
         image="{image}"
         camera_info="{camera_info}"
         base_link="base_link"
-        tag_frame_prefix="calibration_tag_"
+        tag_frame_prefix="{tag_frame_prefix}"
         tag_size="0.051"
         tag_family="tag36h11"
         min_tags="3"
@@ -55,6 +55,7 @@
         name="camera_optical_frame"
         default="{camera_optical_frame}"
       />
+      <inout_port name="tag_frame_prefix" default="calibration_tag_" />
       <inout_port name="computed_pose" default="{computed_pose}" />
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/waypoints/waypoints.yaml b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/waypoints/waypoints.yaml
index 8e11bb5b3..de7a1bab1 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/waypoints/waypoints.yaml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/waypoints/waypoints.yaml
@@ -306,3 +306,187 @@
     twist: []
     wrench: []
   name: View Tag
+- description: Wrist camera views the satellite tags (eye-in-hand cal)
+  favorite: false
+  joint_group_names:
+  - gripper
+  - manipulator
+  joint_state:
+    effort: []
+    header:
+      frame_id: ''
+      stamp:
+        nanosec: 0
+        sec: 0
+    name:
+    - robotiq_85_left_knuckle_joint
+    - robotiq_85_left_finger_joint
+    - robotiq_85_right_finger_joint
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
+    - joint_7
+    position:
+    - 0.0029407721601535414
+    - 0.00014643648808031575
+    - 0.0001464364880981395
+    - -0.7466
+    - 1.5857
+    - -1.3846
+    - -0.0286
+    - -1.2893
+    - -1.9375
+    - 2.5092
+    velocity: []
+  multi_dof_joint_state:
+    header:
+      frame_id: ''
+      stamp:
+        nanosec: 0
+        sec: 0
+    joint_names: []
+    transforms: []
+    twist: []
+    wrench: []
+  name: Wrist Camera Calibration 1
+- description: Wrist camera views the satellite tags (eye-in-hand cal)
+  favorite: false
+  joint_group_names:
+  - gripper
+  - manipulator
+  joint_state:
+    effort: []
+    header:
+      frame_id: ''
+      stamp:
+        nanosec: 0
+        sec: 0
+    name:
+    - robotiq_85_left_knuckle_joint
+    - robotiq_85_left_finger_joint
+    - robotiq_85_right_finger_joint
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
+    - joint_7
+    position:
+    - 0.0029407721601535414
+    - 0.00014643648808031575
+    - 0.0001464364880981395
+    - -0.9087
+    - 1.5934
+    - -1.4064
+    - -0.527
+    - -1.2613
+    - -1.8057
+    - 2.8234
+    velocity: []
+  multi_dof_joint_state:
+    header:
+      frame_id: ''
+      stamp:
+        nanosec: 0
+        sec: 0
+    joint_names: []
+    transforms: []
+    twist: []
+    wrench: []
+  name: Wrist Camera Calibration 2
+- description: Wrist camera views the satellite tags (eye-in-hand cal)
+  favorite: false
+  joint_group_names:
+  - gripper
+  - manipulator
+  joint_state:
+    effort: []
+    header:
+      frame_id: ''
+      stamp:
+        nanosec: 0
+        sec: 0
+    name:
+    - robotiq_85_left_knuckle_joint
+    - robotiq_85_left_finger_joint
+    - robotiq_85_right_finger_joint
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
+    - joint_7
+    position:
+    - 0.0029407721601535414
+    - 0.00014643648808031575
+    - 0.0001464364880981395
+    - -0.7697
+    - 1.5421
+    - -1.3189
+    - -0.0283
+    - -1.3543
+    - -1.9853
+    - 2.5073
+    velocity: []
+  multi_dof_joint_state:
+    header:
+      frame_id: ''
+      stamp:
+        nanosec: 0
+        sec: 0
+    joint_names: []
+    transforms: []
+    twist: []
+    wrench: []
+  name: Wrist Camera Calibration 3
+- description: Wrist camera views the satellite tags (eye-in-hand cal)
+  favorite: false
+  joint_group_names:
+  - gripper
+  - manipulator
+  joint_state:
+    effort: []
+    header:
+      frame_id: ''
+      stamp:
+        nanosec: 0
+        sec: 0
+    name:
+    - robotiq_85_left_knuckle_joint
+    - robotiq_85_left_finger_joint
+    - robotiq_85_right_finger_joint
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
+    - joint_7
+    position:
+    - 0.0029407721601535414
+    - 0.00014643648808031575
+    - 0.0001464364880981395
+    - -0.7479
+    - 1.3555
+    - -1.3291
+    - -0.0284
+    - -1.4385
+    - -2.0564
+    - 2.5514
+    velocity: []
+  multi_dof_joint_state:
+    header:
+      frame_id: ''
+      stamp:
+        nanosec: 0
+        sec: 0
+    joint_names: []
+    transforms: []
+    twist: []
+    wrench: []
+  name: Wrist Camera Calibration 4

From 6565293c4e51489ccb3c81197030d80b2d3e2125 Mon Sep 17 00:00:00 2001
From: Dan Wahl <drwahl@gmail.com>
Date: Thu, 9 Jul 2026 13:25:40 -0500
Subject: [PATCH 4/4] refactor(space_satellite_sim_camera_cal): flat front-face
 wrist-cal tags + diverse waypoints

Mount the three satellite AprilTags flat and coplanar on the servicer's front
(+x) gold face, on the arm-base side of the docking mechanism, replacing the
tilted drum-top placement (scene.xml geoms + matching sat_tag_* URDF frames;
Ry90 so +Z exits the tag face). Regenerate the four wrist-camera waypoints for
diverse, collision-free views that keep all three tags clear of the end
effector. Sim-verified: every waypoint detects all 3 tags and the recovered
wrist mount matches the URDF to ~0.6 mm / 0.02 deg.
---
 .../description/mujoco/scene.xml              | 22 ++++----
 .../description/space_satellite_sim.xacro     | 15 ++---
 .../waypoints/waypoints.yaml                  | 56 +++++++++----------
 3 files changed, 48 insertions(+), 45 deletions(-)

diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/scene.xml b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/scene.xml
index c7d839190..21e98ca67 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/scene.xml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/scene.xml
@@ -185,18 +185,20 @@
 
     <!--
       Satellite calibration tags for the WRIST camera (eye-in-hand). Three tag36h11
-      markers (ids 3,4,5) on the drum top, tilted ~30 deg about Y (quat = Ry30) so the
-      tag face angles toward the servicer arm. 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.
+      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.9659258 0 0.2588190 0"
+        quat="0.7071068 0 0.7071068 0"
         size="0.031875 0.031875 0.0002"
-        pos="0.42 0.46 0.274"
+        pos="0.50 0.45 0.04"
         rgba="1 1 1 1"
         material="ar_marker_3"
         contype="0"
@@ -205,9 +207,9 @@
       <geom
         name="sat_tag_4"
         type="box"
-        quat="0.9659258 0 0.2588190 0"
+        quat="0.7071068 0 0.7071068 0"
         size="0.031875 0.031875 0.0002"
-        pos="0.42 0.66 0.274"
+        pos="0.50 0.27 0.04"
         rgba="1 1 1 1"
         material="ar_marker_4"
         contype="0"
@@ -216,9 +218,9 @@
       <geom
         name="sat_tag_5"
         type="box"
-        quat="0.9659258 0 0.2588190 0"
+        quat="0.7071068 0 0.7071068 0"
         size="0.031875 0.031875 0.0002"
-        pos="0.20 0.56 0.274"
+        pos="0.50 0.36 -0.12"
         rgba="1 1 1 1"
         material="ar_marker_5"
         contype="0"
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/space_satellite_sim.xacro b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/space_satellite_sim.xacro
index 374daa4a6..561ae4a3b 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/space_satellite_sim.xacro
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/space_satellite_sim.xacro
@@ -630,27 +630,28 @@
   </joint>
 
   <!-- Satellite calibration tags (static, world-fixed) for the wrist camera. tag36h11
-       ids 3/4/5, mounted as angled plates on the drum top facing the servicer arm; the
-       rpy tilt (Ry 30 deg) points +Z out of the tag face 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>'. -->
+       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.42 0.46 0.274" rpy="0 0.5236 0" />
+    <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.42 0.66 0.274" rpy="0 0.5236 0" />
+    <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.20 0.56 0.274" rpy="0 0.5236 0" />
+    <origin xyz="0.50 0.36 -0.12" rpy="0 1.5708 0" />
   </joint>
 
   <ros2_control name="kinova_mujoco_simulation" type="system">
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/waypoints/waypoints.yaml b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/waypoints/waypoints.yaml
index de7a1bab1..5981fe3b3 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/waypoints/waypoints.yaml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/waypoints/waypoints.yaml
@@ -333,13 +333,13 @@
     - 0.0029407721601535414
     - 0.00014643648808031575
     - 0.0001464364880981395
-    - -0.7466
-    - 1.5857
-    - -1.3846
-    - -0.0286
-    - -1.2893
-    - -1.9375
-    - 2.5092
+    - -0.2415
+    - 0.4937
+    - -1.3862
+    - -2.1321
+    - -0.9097
+    - -1.5143
+    - -0.4093
     velocity: []
   multi_dof_joint_state:
     header:
@@ -379,13 +379,13 @@
     - 0.0029407721601535414
     - 0.00014643648808031575
     - 0.0001464364880981395
-    - -0.9087
-    - 1.5934
-    - -1.4064
-    - -0.527
-    - -1.2613
-    - -1.8057
-    - 2.8234
+    - -0.9033
+    - 1.1219
+    - -1.2598
+    - -1.8335
+    - -0.9293
+    - -1.5142
+    - -1.0942
     velocity: []
   multi_dof_joint_state:
     header:
@@ -425,13 +425,13 @@
     - 0.0029407721601535414
     - 0.00014643648808031575
     - 0.0001464364880981395
-    - -0.7697
-    - 1.5421
-    - -1.3189
-    - -0.0283
-    - -1.3543
-    - -1.9853
-    - 2.5073
+    - -1.2524
+    - 1.0288
+    - -0.8622
+    - -2.4734
+    - -1.1231
+    - -1.2013
+    - -0.5478
     velocity: []
   multi_dof_joint_state:
     header:
@@ -471,13 +471,13 @@
     - 0.0029407721601535414
     - 0.00014643648808031575
     - 0.0001464364880981395
-    - -0.7479
-    - 1.3555
-    - -1.3291
-    - -0.0284
-    - -1.4385
-    - -2.0564
-    - 2.5514
+    - -0.2284
+    - 0.862
+    - -1.5603
+    - -1.5036
+    - -0.93
+    - -1.8527
+    - -0.8653
     velocity: []
   multi_dof_joint_state:
     header: