<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="zivid_camera_mount">
<!--
Zivid Camera Mount with Direct Hand-Eye Calibration
Frame chain:
parent_link (tool0) -> zivid_optical_frame (CALIBRATED - point cloud origin)
parent_link (tool0) -> zivid_arm_mount -> zivid_base_link (visual mesh only)
The hand-eye calibration gives us: tool0 -> optical_frame directly.
We DON'T use the zivid_camera macro's uncalibrated optical joint.
-->
<xacro:macro name="zivid_camera_mount" params="parent_link xyz:='0 0 0' rpy:='0 0 0' zivid_model:='ZIVID_2_PLUS_MR60'">
<!-- ============================================================ -->
<!-- CALIBRATED OPTICAL FRAME (for point cloud data) -->
<!-- This is where the Zivid point cloud is referenced -->
<!-- Hand-eye calibration result: tool0 -> zivid_optical_frame -->
<!-- ============================================================ -->
<link name="zivid_optical_frame"/>
<joint name="tool0_to_zivid_optical" type="fixed">
<parent link="${parent_link}"/>
<child link="zivid_optical_frame"/>
<!-- Hand-eye calibration: tool0 -> optical_frame -->
<!-- Rotation: Euler XYZ extrinsic, radians | Translation: meters -->
<!-- Calibration (2026-07-22): Zivid Studio GUI hand-eye recalibration (4x4 in hand_eye_calibration.yaml). Pending touch-test verification. -->
<origin xyz="0.05594 0.10469 0.05890"
rpy="-0.03511 0.05081 3.13360"/>
<!-- Calibration (2026-03-27): 12 poses (pose 4 excluded as outlier). Residuals: rot < 0.33°, trans < 1.53mm
<origin xyz="0.05635 0.10228 0.06025"
rpy="-0.03622 0.05218 3.13437"/> -->
<!-- Calibration (2026-03-25): Residuals: rot < 0.15°, trans < 1.0mm
<origin xyz="0.05475 0.10491 0.06013"
rpy="-0.03489 0.05317 3.13637"/> -->
<!-- Calibration (2026-01-15): Residuals: rot < 0.22°, trans < 0.47mm
<origin xyz="0.05675 0.10322 0.05489"
rpy="-0.00615 0.04362 3.13541"/> -->
<!-- TEST (2026-01-22): Zero roll/pitch - did NOT help Y variance
<origin xyz="0.05675 0.10322 0.05489"
rpy="0 0 3.14159"/> -->
<!-- Calibration (2025-12-17):
<origin xyz="0.05659 0.10548 0.05660"
rpy="-0.01432 0.04829 3.13430"/> -->
<!-- Calibration (2026-01-13) - had offset issues:
<origin xyz="0.05646 0.10182 0.05680"
rpy="-0.03542 0.04745 3.13222"/> -->
<!-- Old calibration (2025-10-09):
<origin xyz="0.02803 0.07664 0.0"
rpy="0.53964 -1.53712 -2.13794"/> -->
</joint>
<!-- ============================================================ -->
<!-- ARM MOUNT (for attaching grippers via wrist_extension) -->
<!-- ============================================================ -->
<link name="zivid_arm_mount">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<!-- Official Zivid On-Arm Mount (from zivid.com/downloads) -->
<mesh filename="package://cms_robot_description/meshes/zivid/zivid_onarm_mount.stl"
scale="0.001 0.001 0.001"/>
</geometry>
<material name="mount_dark">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://cms_robot_description/meshes/zivid/zivid_onarm_mount.stl"
scale="0.001 0.001 0.001"/>
</geometry>
</collision>
<inertial>
<mass value="0.170"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="0.0003" ixy="0.0" ixz="0.0"
iyy="0.0005" iyz="0.0"
izz="0.0006"/>
</inertial>
</link>
<joint name="zivid_arm_mount_joint" type="fixed">
<parent link="${parent_link}"/>
<child link="zivid_arm_mount"/>
<origin xyz="${xyz}" rpy="${rpy}"/>
</joint>
<!-- ============================================================ -->
<!-- CAMERA VISUAL MESH (for RViz visualization) -->
<!-- Positioned relative to optical frame using inverse offset -->
<!-- ============================================================ -->
<link name="zivid_base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 ${0.5*pi}"/>
<geometry>
<mesh filename="package://zivid_description/meshes/visual/zivid_2_plus.stl"/>
</geometry>
<material name="zivid_gray">
<color rgba="0.3 0.3 0.3 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 ${0.5*pi}"/>
<geometry>
<mesh filename="package://zivid_description/meshes/collision/zivid_2_plus_convex_hull.stl"/>
</geometry>
</collision>
</link>
<!-- Position visual mesh relative to optical frame -->
<!-- INVERSE of Zivid's factory transform (base_link → optical_frame) -->
<!-- Source: zivid_description/urdf/macros/zivid_camera.xacro (ZIVID_2_PLUS_MR60) -->
<!-- Forward: xyz=(0.049, 0.03202, 0.0295), rpy=(-90°, 0°, -92.5°) -->
<!-- Inverse: T_inv = [R^T | -R^T * t] computed mathematically -->
<joint name="optical_to_base_link" type="fixed">
<parent link="zivid_optical_frame"/>
<child link="zivid_base_link"/>
<origin xyz="0.03413 0.02950 -0.04756"
rpy="-1.57080 -1.52716 3.14159"/>
</joint>
</xacro:macro>
</robot>