From 97d489dd9f8b290f499cd26e9c6bea553454423c Mon Sep 17 00:00:00 2001
From: Rooholla-Khorrambakht <learn.kho@gmail.com>
Date: Mon, 27 May 2024 23:22:10 -0400
Subject: [PATCH] utlidar extrinsics updated

---
 Go2Py/assets/urdf/go2.urdf | 72 ++++++++++++++++++++++++++++++++++++--
 1 file changed, 69 insertions(+), 3 deletions(-)

diff --git a/Go2Py/assets/urdf/go2.urdf b/Go2Py/assets/urdf/go2.urdf
index 7cd83f4..06a104a 100644
--- a/Go2Py/assets/urdf/go2.urdf
+++ b/Go2Py/assets/urdf/go2.urdf
@@ -665,17 +665,83 @@ Stephen Brawner (brawner@gmail.com)
     <child link="imu_link"/>
     <axis xyz="0 0 0"/>
   </joint>
-  <link name="radar">
+
+  <joint name="vicon_joint" type="fixed">
+        <parent link="trunk"/>
+        <child link="vicon_link"/>
+        <origin rpy="-0.037935 -0.066371 -2.668897" xyz="0.022944 -0.000007 0.058458"/>
+    </joint>
+
+    <link name="vicon_link">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="0.001 0.001 0.001"/>
+            </geometry>
+            <!-- <material name="red"/> -->
+        </visual>
+    </link>
+
+
+    <joint name="infra1_joint" type="fixed">
+        <parent link="trunk"/>
+        <child link="infra1_link"/>
+        <origin rpy="-1.641669 -0.014332 -1.617155" xyz="0.321618 0.033305 0.081622"/>
+    </joint>
+
+    <link name="infra1_link">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="0.001 0.001 0.001"/>
+            </geometry>
+            <!-- <material name="red"/> -->
+        </visual>
+    </link>
+
+    <joint name="infra2_joint" type="fixed">
+        <parent link="trunk"/>
+        <child link="infra1_link"/>
+        <origin rpy="-1.640637 -0.014093 -1.611177" xyz="0.317969 -0.061919 0.082773"/>
+    </joint>
+
+    <link name="infra2_link">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="0.001 0.001 0.001"/>
+            </geometry>
+            <!-- <material name="red"/> -->
+        </visual>
+    </link>
+
+    <joint name="color_joint" type="fixed">
+        <parent link="trunk"/>
+        <child link="color_link"/>
+        <origin rpy="-1.640491 -0.011125 -1.610364" xyz="0.319415 -0.025790 0.082311"/>
+    </joint>
+
+    <link name="color_link">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="0.001 0.001 0.001"/>
+            </geometry>
+            <!-- <material name="red"/> -->
+        </visual>
+    </link>
+
+  <link name="utlidar">
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <mass value="0"/>
       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
     </inertial>
   </link>
-  <joint name="radar_joint" type="fixed">
+  <joint name="utlidar_joint" type="fixed">
     <origin rpy="0 2.8782 0" xyz="0.28945 0 -0.046825"/>
     <parent link="base_link"/>
-    <child link="radar"/>
+    <child link="utlidar"/>
     <axis xyz="0 0 0"/>
   </joint>
 </robot>