Fix 2cm baselink offset, update servicer cam to realsense.

rlpratt12 · rlpratt12 · commit 84aa91794231 · 2025-04-28T17:18:59.000-06:00
diff --git a/src/calibration_behaviors/src/save_calibration_pose_yaml.cpp b/src/calibration_behaviors/src/save_calibration_pose_yaml.cpp
@@ -58,16 +58,6 @@ BT::NodeStatus SaveCalibrationPoseYaml::tick()
   double roll, pitch, yaw;
   tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);
 
-  // Create the YAML to save.
-  YAML::Node node;
-  std::string calibration_data_str = "camera_pose";
-  node[calibration_data_str]["x"] = pose_stamped.pose.position.x;
-  node[calibration_data_str]["y"] = pose_stamped.pose.position.y;
-  node[calibration_data_str]["z"] = pose_stamped.pose.position.z;
-  node[calibration_data_str]["roll"] = roll;
-  node[calibration_data_str]["pitch"] = pitch;
-  node[calibration_data_str]["yaw"] = yaw;
-
   // Attempt to save the file.
   const std::string objective_source_directory =
       shared_resources_->node->get_parameter("config_source_directory").as_string() + "/objectives";
@@ -82,7 +72,9 @@ BT::NodeStatus SaveCalibrationPoseYaml::tick()
   shared_resources_->logger->publishInfoMessage(
       fmt::format("Writing calibration file to '{}'", filepath_maybe.value().string()));
   std::ofstream file_out(filepath_maybe.value());
-  file_out << node;
+  // Format for raw copy to .xacro
+  file_out << "<origin xyz=\" " << pose_stamped.pose.position.x << " " << pose_stamped.pose.position.y << " "
+           << pose_stamped.pose.position.z << "\" rpy=\"" << roll << " " << pitch << " " << yaw << "\" />";
   file_out.close();
 
   return BT::NodeStatus::SUCCESS;
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/gen3_7dof_body.xml b/src/moveit_pro_kinova_configs/space_satellite_sim_camera_cal/description/mujoco/gen3_7dof_body.xml
@@ -1,5 +1,5 @@
 <mujocoinclude>
-  <body name="base_link" gravcomp="1" pos="0 0 0.02" quat="1 0 0 0">
+  <body name="base_link" gravcomp="1" pos="0 0 0" quat="1 0 0 0">
     <inertial
       pos="-0.000648 -0.000166 0.084487"
       quat="1 0 0 0"
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
@@ -103,12 +103,6 @@
         euler="1.5708 -2 0.0"
         user="1"
       />
-      <site
-        name="servicer_camera_mounting_frame"
-        pos="0.6 1 0"
-        euler="1.5708 -2 0.0"
-        size="0.03"
-      />
       <site
         name="servicer_camera_optical_frame"
         pos="0.6 1 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
@@ -555,9 +555,9 @@
     </visual>
   </link>
   <joint name="calibration_tool_joint" type="fixed">
-    <parent link="grasp_link"/>
+    <parent link="robotiq_85_base_link"/>
     <child link="calibration_tool_link"/>
-    <origin xyz="0 0 0.019" rpy="0 0 0"/>
+    <origin xyz="0 0 0.169" rpy="0 0 0"/>
   </joint>
   <link name="calibration_tag_right">
     <visual>
@@ -607,15 +607,27 @@
     <origin xyz="0 -0.09325 -0.01" rpy="0 ${pi} ${pi}"/>
   </joint>
 
-  <!-- Camera External -->
-  <link name="external_camera_link" />
-  <joint name="external_camera_joint" type="fixed">
+  <!-- Servicer Camera -->
+  <link name="servicer_camera_mount_link" />
+  <joint name="servicer_camera_mount_joint" type="fixed">
+    <parent link="world" />
+    <child link="servicer_camera_mount_link" />
+    <origin xyz=" 0.585021 0.966828 -0.0111449" rpy="0.00122423 0.00171265 -0.426171" />
+  </joint>
+
+  <xacro:realsense_d415 parent="servicer_camera_mount_link" name="servicer_camera">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </xacro:realsense_d415>
+
+  <!-- Scene Camera -->
+  <link name="scene_camera_mount_link" />
+  <joint name="scene_camera_mount_joint" type="fixed">
     <parent link="world" />
-    <child link="external_camera_link" />
+    <child link="scene_camera_mount_link" />
     <origin xyz="-0.3 0.3 1.0" rpy="0 0.5236 0" />
   </joint>
 
-  <xacro:realsense_d415 parent="external_camera_link" name="scene_camera">
+  <xacro:realsense_d415 parent="scene_camera_mount_link" name="scene_camera">
     <origin xyz="0 0 0" rpy="0 0 0" />
   </xacro:realsense_d415>
 
