From f05ab8fe0178216ad951743c97958d3f7d3ffbc6 Mon Sep 17 00:00:00 2001
From: Felix Maisonneuve <fmaisonneuve@kinova.ca>
Date: Mon, 3 Jan 2022 14:03:19 -0500
Subject: [PATCH 1/2] added links for vision in 6dof description

---
 .../arms/gen3/6dof/urdf/gen3_macro.xacro      | 20 +++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro b/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro
index c12d737b..6096d234 100644
--- a/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro
+++ b/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro
@@ -240,5 +240,25 @@
       <axis
         xyz="0 0 0" />
     </joint>
+    <!-- <xacro:if value="${vision}">
+      <link name="${prefix}camera_link" />
+      <joint name="${prefix}camera_module" type="fixed">
+        <origin xyz="0 0.05639 -0.00305" rpy="3.14159265358979 3.14159265358979 0" />
+        <parent link="${prefix}end_effector_link" />
+        <child  link="${prefix}camera_link" />
+      </joint>
+      <link name="${prefix}camera_depth_frame" />
+      <joint name="${prefix}depth_module" type="fixed">
+        <origin xyz="0.0275 0.066 -0.00305" rpy="3.14159265358979 3.14159265358979 0" />
+        <parent link="${prefix}end_effector_link" />
+        <child  link="${prefix}camera_depth_frame" />
+      </joint>
+      <link name="${prefix}camera_color_frame" />
+      <joint name="${prefix}color_module" type="fixed">
+        <origin xyz="0 0.05639 -0.00305" rpy="3.14159265358979 3.14159265358979 0" />
+        <parent link="${prefix}end_effector_link" />
+        <child  link="${prefix}camera_color_frame" />
+      </joint>
+    </xacro:if> -->
   </xacro:macro>
 </robot>

From 9d789db49be6431ba2198416c33adacea79ca4f9 Mon Sep 17 00:00:00 2001
From: Felix Maisonneuve <fmaisonneuve@kinova.ca>
Date: Mon, 3 Jan 2022 14:04:22 -0500
Subject: [PATCH 2/2] uncommented it

---
 kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro b/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro
index 6096d234..d409a35a 100644
--- a/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro
+++ b/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro
@@ -240,7 +240,7 @@
       <axis
         xyz="0 0 0" />
     </joint>
-    <!-- <xacro:if value="${vision}">
+    <xacro:if value="${vision}">
       <link name="${prefix}camera_link" />
       <joint name="${prefix}camera_module" type="fixed">
         <origin xyz="0 0.05639 -0.00305" rpy="3.14159265358979 3.14159265358979 0" />
@@ -259,6 +259,6 @@
         <parent link="${prefix}end_effector_link" />
         <child  link="${prefix}camera_color_frame" />
       </joint>
-    </xacro:if> -->
+    </xacro:if>
   </xacro:macro>
 </robot>