From 43dc42742aa470b921ec647955ef3d888c6f9850 Mon Sep 17 00:00:00 2001
From: Katie Hughes <khughes-bdai@theaiinstitute.com>
Date: Tue, 29 Oct 2024 11:33:19 -0400
Subject: [PATCH 1/5] Baseline gripperless parameter

---
 spot_description/urdf/spot.urdf.xacro     |   2 +
 spot_description/urdf/spot_arm_macro.urdf | 120 +++++++++++-----------
 spot_description/urdf/spot_macro.xacro    |   3 +-
 3 files changed, 65 insertions(+), 60 deletions(-)

diff --git a/spot_description/urdf/spot.urdf.xacro b/spot_description/urdf/spot.urdf.xacro
index 5fb809923..90f58519d 100644
--- a/spot_description/urdf/spot.urdf.xacro
+++ b/spot_description/urdf/spot.urdf.xacro
@@ -8,6 +8,7 @@
 
   <!-- General parameters -->
   <xacro:arg name="arm" default="false" />
+  <xacro:arg name="gripperless" default="false" />
   <xacro:arg name="feet" default="false" />
   <xacro:arg name="tf_prefix" default="" />
 
@@ -23,6 +24,7 @@
   <!-- Load Spot -->
   <xacro:load_spot
     arm="$(arg arm)"
+    gripperless="$(arg gripperless)"
     feet="$(arg feet)"
     tf_prefix="$(arg tf_prefix)" />
 
diff --git a/spot_description/urdf/spot_arm_macro.urdf b/spot_description/urdf/spot_arm_macro.urdf
index 146c9bca3..e8ae971a8 100644
--- a/spot_description/urdf/spot_arm_macro.urdf
+++ b/spot_description/urdf/spot_arm_macro.urdf
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <robot name="spot_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
   <xacro:macro name="load_arm" params="
-    tf_prefix">
+    tf_prefix gripperless">
 
     <link name="${tf_prefix}arm_link_sh0">
       <visual>
@@ -217,9 +217,9 @@
         </geometry>
       </collision>
       <collision>
-      <geometry>
-        <mesh filename="package://spot_description/meshes/arm/collision/front_jaw.obj" />
-      </geometry>
+        <geometry>
+          <mesh filename="package://spot_description/meshes/arm/collision/front_jaw.obj" />
+        </geometry>
       </collision>
       <collision>
         <geometry>
@@ -248,60 +248,62 @@
         upper="2.87979326579064354163" />
     </joint>
 
-    <link name="${tf_prefix}arm_link_fngr">
-      <visual>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <mesh filename="package://spot_description/meshes/arm/visual/arm_link_fngr.obj" />
-        </geometry>
-        <material name="arm_link_fngr">
-          <color rgba="0.2 0.2 0.2 1" />
-        </material>
-      </visual>
-      <collision>
-        <geometry>
-          <mesh filename="package://spot_description/meshes/arm/collision/left_hinge.obj" />
-        </geometry>
-      </collision>
-      <collision>
-        <geometry>
-          <mesh filename="package://spot_description/meshes/arm/collision/left_finger.obj" />
-        </geometry>
-      </collision>
-      <collision>
-        <geometry>
-          <mesh filename="package://spot_description/meshes/arm/collision/left_tooth.obj" />
-        </geometry>
-      </collision>
-      <collision>
-        <geometry>
-          <mesh filename="package://spot_description/meshes/arm/collision/right_hinge.obj" />
-        </geometry>
-      </collision>
-      <collision>
-        <geometry>
-          <mesh filename="package://spot_description/meshes/arm/collision/right_finger.obj" />
-        </geometry>
-      </collision>
-      <collision>
-        <geometry>
-          <mesh filename="package://spot_description/meshes/arm/collision/right_tooth.obj" />
-        </geometry>
-      </collision>
-      <inertial>
-        <origin rpy="0.0003416659657240473 0.3751670012784243 -0.004058661249495721"
-          xyz="0.04787873849272728 -0.00020676758140325546 -0.01628788933157921" />
-        <mass value="0.2" />
-        <inertia ixx="0.0001439963634171048" ixy="0.0" ixz="0.0" iyy="0.0003007126997998294"
-          iyz="0.0" izz="0.0003912783268020657" />
-      </inertial>
-    </link>
-    <joint name="${tf_prefix}arm_f1x" type="revolute">
-      <origin xyz="0.11745 0 0.014820" rpy="0 0 0" />
-      <axis xyz="0.0 1.0 0.0" />
-      <parent link="${tf_prefix}arm_link_wr1" />
-      <child link="${tf_prefix}arm_link_fngr" />
-      <limit effort="15.32" velocity="10.0" lower="-1.57" upper="0.0" />
-    </joint>
+    <xacro:unless value="${gripperless}">
+      <link name="${tf_prefix}arm_link_fngr">
+        <visual>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://spot_description/meshes/arm/visual/arm_link_fngr.obj" />
+          </geometry>
+          <material name="arm_link_fngr">
+            <color rgba="0.2 0.2 0.2 1" />
+          </material>
+        </visual>
+        <collision>
+          <geometry>
+            <mesh filename="package://spot_description/meshes/arm/collision/left_hinge.obj" />
+          </geometry>
+        </collision>
+        <collision>
+          <geometry>
+            <mesh filename="package://spot_description/meshes/arm/collision/left_finger.obj" />
+          </geometry>
+        </collision>
+        <collision>
+          <geometry>
+            <mesh filename="package://spot_description/meshes/arm/collision/left_tooth.obj" />
+          </geometry>
+        </collision>
+        <collision>
+          <geometry>
+            <mesh filename="package://spot_description/meshes/arm/collision/right_hinge.obj" />
+          </geometry>
+        </collision>
+        <collision>
+          <geometry>
+            <mesh filename="package://spot_description/meshes/arm/collision/right_finger.obj" />
+          </geometry>
+        </collision>
+        <collision>
+          <geometry>
+            <mesh filename="package://spot_description/meshes/arm/collision/right_tooth.obj" />
+          </geometry>
+        </collision>
+        <inertial>
+          <origin rpy="0.0003416659657240473 0.3751670012784243 -0.004058661249495721"
+            xyz="0.04787873849272728 -0.00020676758140325546 -0.01628788933157921" />
+          <mass value="0.2" />
+          <inertia ixx="0.0001439963634171048" ixy="0.0" ixz="0.0" iyy="0.0003007126997998294"
+            iyz="0.0" izz="0.0003912783268020657" />
+        </inertial>
+      </link>
+      <joint name="${tf_prefix}arm_f1x" type="revolute">
+        <origin xyz="0.11745 0 0.014820" rpy="0 0 0" />
+        <axis xyz="0.0 1.0 0.0" />
+        <parent link="${tf_prefix}arm_link_wr1" />
+        <child link="${tf_prefix}arm_link_fngr" />
+        <limit effort="15.32" velocity="10.0" lower="-1.57" upper="0.0" />
+      </joint>
+    </xacro:unless>
   </xacro:macro>
 </robot>
diff --git a/spot_description/urdf/spot_macro.xacro b/spot_description/urdf/spot_macro.xacro
index 5f947457c..1b99c6af4 100644
--- a/spot_description/urdf/spot_macro.xacro
+++ b/spot_description/urdf/spot_macro.xacro
@@ -4,6 +4,7 @@
     <xacro:macro name="load_spot" params="
     arm:=false
     feet:=false
+    gripperless:=false
     tf_prefix">
 
         <link name="${tf_prefix}body">
@@ -417,7 +418,7 @@
         <!-- Include Arm if necessary-->
         <xacro:if value="${arm}">
             <xacro:include filename="$(find spot_description)/urdf/spot_arm_macro.urdf" />
-            <xacro:load_arm tf_prefix="${tf_prefix}" />
+            <xacro:load_arm tf_prefix="${tf_prefix}" gripperless="${gripperless}" />
         </xacro:if>
 
         <!-- Include links at feet if necessary -->

From 7d2e56d65f7931867379d5ae4d90673a33bc4d5e Mon Sep 17 00:00:00 2001
From: Katie Hughes <khughes-bdai@theaiinstitute.com>
Date: Wed, 30 Oct 2024 16:11:57 -0400
Subject: [PATCH 2/5] Fix for stray meshes in WR1 link

---
 spot_description/launch/description.launch.py |  8 ++
 spot_description/urdf/spot_arm_macro.urdf     | 83 ++++++++++---------
 2 files changed, 53 insertions(+), 38 deletions(-)

diff --git a/spot_description/launch/description.launch.py b/spot_description/launch/description.launch.py
index bd1ea47b5..0398d0a75 100644
--- a/spot_description/launch/description.launch.py
+++ b/spot_description/launch/description.launch.py
@@ -39,6 +39,12 @@ def generate_launch_description() -> launch.LaunchDescription:
                 choices=["True", "true", "False", "false"],
                 description="Flag to enable putting frames at the feet",
             ),
+            DeclareLaunchArgument(
+                name="gripperless",
+                default_value="False",
+                choices=["True", "true", "False", "false"],
+                description="Flag to remove the gripper from the arm model",
+            ),
             DeclareLaunchArgument(
                 "tf_prefix", default_value='""', description="Apply namespace prefix to robot links and joints"
             ),
@@ -56,6 +62,8 @@ def generate_launch_description() -> launch.LaunchDescription:
                                 LaunchConfiguration("arm"),
                                 " feet:=",
                                 LaunchConfiguration("feet"),
+                                " gripperless:=",
+                                LaunchConfiguration("gripperless"),
                                 " tf_prefix:=",
                                 LaunchConfiguration("tf_prefix"),
                             ]
diff --git a/spot_description/urdf/spot_arm_macro.urdf b/spot_description/urdf/spot_arm_macro.urdf
index e8ae971a8..f7e7aa923 100644
--- a/spot_description/urdf/spot_arm_macro.urdf
+++ b/spot_description/urdf/spot_arm_macro.urdf
@@ -201,44 +201,51 @@
         upper="1.83259571459404613236" />
     </joint>
 
-    <link name="${tf_prefix}arm_link_wr1">
-      <visual>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <mesh filename="package://spot_description/meshes/arm/visual/arm_link_wr1.obj" />
-        </geometry>
-        <material name="arm_link_wr1">
-          <color rgba="0.2 0.2 0.2 1" />
-        </material>
-      </visual>
-      <collision>
-        <geometry>
-          <mesh filename="package://spot_description/meshes/arm/collision/arm_link_wr1.obj" />
-        </geometry>
-      </collision>
-      <collision>
-        <geometry>
-          <mesh filename="package://spot_description/meshes/arm/collision/front_jaw.obj" />
-        </geometry>
-      </collision>
-      <collision>
-        <geometry>
-          <mesh filename="package://spot_description/meshes/arm/collision/middle_jaw.obj" />
-        </geometry>
-      </collision>
-      <collision>
-        <geometry>
-          <mesh filename="package://spot_description/meshes/arm/collision/jaw_tooth.obj" />
-        </geometry>
-      </collision>
-      <inertial>
-        <origin rpy="0.006393076449033863 0.2928526604761865 -0.022814984050994802"
-          xyz="0.12516802549362183 0.00010137435310753062 -0.013997982256114483" />
-        <mass value="0.785" />
-        <inertia ixx="0.00076741362048185" ixy="0.0" ixz="0.0" iyy="0.0017628124054271809" iyz="0.0"
-          izz="0.0016818105865345951" />
-      </inertial>
-    </link>
+    <xacro:if value="${gripperless}">
+      <!-- This link needs to still be defined as to not break WR1 joint but can't include the meshes below, as these
+      includes parts of the gripper. -->
+      <link name="${tf_prefix}arm_link_wr1" />
+    </xacro:if>
+    <xacro:unless value="${gripperless}">
+      <link name="${tf_prefix}arm_link_wr1">
+        <visual>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://spot_description/meshes/arm/visual/arm_link_wr1.obj" />
+          </geometry>
+          <material name="arm_link_wr1">
+            <color rgba="0.2 0.2 0.2 1" />
+          </material>
+        </visual>
+        <collision>
+          <geometry>
+            <mesh filename="package://spot_description/meshes/arm/collision/arm_link_wr1.obj" />
+          </geometry>
+        </collision>
+        <collision>
+          <geometry>
+            <mesh filename="package://spot_description/meshes/arm/collision/front_jaw.obj" />
+          </geometry>
+        </collision>
+        <collision>
+          <geometry>
+            <mesh filename="package://spot_description/meshes/arm/collision/middle_jaw.obj" />
+          </geometry>
+        </collision>
+        <collision>
+          <geometry>
+            <mesh filename="package://spot_description/meshes/arm/collision/jaw_tooth.obj" />
+          </geometry>
+        </collision>
+        <inertial>
+          <origin rpy="0.006393076449033863 0.2928526604761865 -0.022814984050994802"
+            xyz="0.12516802549362183 0.00010137435310753062 -0.013997982256114483" />
+          <mass value="0.785" />
+          <inertia ixx="0.00076741362048185" ixy="0.0" ixz="0.0" iyy="0.0017628124054271809" iyz="0.0"
+            izz="0.0016818105865345951" />
+        </inertial>
+      </link>
+    </xacro:unless>
     <joint name="${tf_prefix}arm_wr1" type="revolute">
       <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
       <axis xyz="1 0 0" />

From d16078a0a819a213877099c3ae20fd94224c1365 Mon Sep 17 00:00:00 2001
From: Katie Hughes <khughes-bdai@theaiinstitute.com>
Date: Wed, 30 Oct 2024 17:18:27 -0400
Subject: [PATCH 3/5] seems to be working correctly

---
 spot_description/urdf/spot.urdf.xacro     | 10 +++++++---
 spot_description/urdf/spot_arm_macro.urdf | 24 ++++++++++++++++++-----
 spot_description/urdf/spot_macro.xacro    |  5 ++++-
 3 files changed, 30 insertions(+), 9 deletions(-)

diff --git a/spot_description/urdf/spot.urdf.xacro b/spot_description/urdf/spot.urdf.xacro
index 90f58519d..760a49bf1 100644
--- a/spot_description/urdf/spot.urdf.xacro
+++ b/spot_description/urdf/spot.urdf.xacro
@@ -8,10 +8,13 @@
 
   <!-- General parameters -->
   <xacro:arg name="arm" default="false" />
-  <xacro:arg name="gripperless" default="false" />
   <xacro:arg name="feet" default="false" />
   <xacro:arg name="tf_prefix" default="" />
 
+  <!-- Parameters used if Spot has an arm but not a gripper, or is outfitted with a custom gripper. -->
+  <xacro:arg name="gripperless" default="false" />
+  <xacro:arg name="custom_gripper_base_link" default="" />
+
   <!-- Parameters for ROS 2 control -->
   <xacro:arg name="add_ros2_control_tag" default="false" />
   <xacro:arg name="hardware_interface_type" default="mock" />
@@ -24,9 +27,10 @@
   <!-- Load Spot -->
   <xacro:load_spot
     arm="$(arg arm)"
-    gripperless="$(arg gripperless)"
     feet="$(arg feet)"
-    tf_prefix="$(arg tf_prefix)" />
+    tf_prefix="$(arg tf_prefix)" 
+    gripperless="$(arg gripperless)"
+    custom_gripper_base_link="$(arg custom_gripper_base_link)"/>
 
   <!-- Adding the ROS 2 control tags -->
   <xacro:if value="$(arg add_ros2_control_tag)">
diff --git a/spot_description/urdf/spot_arm_macro.urdf b/spot_description/urdf/spot_arm_macro.urdf
index f7e7aa923..f15882bcc 100644
--- a/spot_description/urdf/spot_arm_macro.urdf
+++ b/spot_description/urdf/spot_arm_macro.urdf
@@ -1,7 +1,10 @@
 <?xml version="1.0"?>
 <robot name="spot_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
   <xacro:macro name="load_arm" params="
-    tf_prefix gripperless">
+    tf_prefix gripperless custom_gripper_base_link">
+
+    <!-- Useful boolean variable for determining if custom gripper base link should be used in the xacro -->
+    <xacro:property name="use_custom_gripper_base_link" value="${gripperless and custom_gripper_base_link!=''}"/>
 
     <link name="${tf_prefix}arm_link_sh0">
       <visual>
@@ -202,9 +205,11 @@
     </joint>
 
     <xacro:if value="${gripperless}">
-      <!-- This link needs to still be defined as to not break WR1 joint but can't include the meshes below, as these
-      includes parts of the gripper. -->
-      <link name="${tf_prefix}arm_link_wr1" />
+      <xacro:unless value="${use_custom_gripper_base_link}">
+        <!-- Here this link needs to still be defined as to not break WR1 joint but can't include the meshes below, as
+        these include parts of the gripper. Otherwise custom gripper base link will be used as the child of WR1 -->
+        <link name="${tf_prefix}arm_link_wr1" />
+      </xacro:unless>
     </xacro:if>
     <xacro:unless value="${gripperless}">
       <link name="${tf_prefix}arm_link_wr1">
@@ -250,11 +255,20 @@
       <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
       <axis xyz="1 0 0" />
       <parent link="${tf_prefix}arm_link_wr0" />
-      <child link="${tf_prefix}arm_link_wr1" />
+      <!-- Child link can be different depending on gripperless status: -->
+      <xacro:if value="${use_custom_gripper_base_link}">
+        <!-- can optionally be the custom link specified via arg -->
+        <child link="${custom_gripper_base_link}" />
+      </xacro:if>
+      <xacro:unless value="${use_custom_gripper_base_link}">
+        <!-- else defaults to normal arm_link_wr1 -->
+        <child link="${tf_prefix}arm_link_wr1" />
+      </xacro:unless>
       <limit effort="30.3" velocity="10.0" lower="-2.87979326579064354163"
         upper="2.87979326579064354163" />
     </joint>
 
+    <!-- Finger joint and link shouldn't be included if gripperless -->
     <xacro:unless value="${gripperless}">
       <link name="${tf_prefix}arm_link_fngr">
         <visual>
diff --git a/spot_description/urdf/spot_macro.xacro b/spot_description/urdf/spot_macro.xacro
index 1b99c6af4..fd36ad15c 100644
--- a/spot_description/urdf/spot_macro.xacro
+++ b/spot_description/urdf/spot_macro.xacro
@@ -5,6 +5,7 @@
     arm:=false
     feet:=false
     gripperless:=false
+    custom_gripper_base_link
     tf_prefix">
 
         <link name="${tf_prefix}body">
@@ -418,7 +419,9 @@
         <!-- Include Arm if necessary-->
         <xacro:if value="${arm}">
             <xacro:include filename="$(find spot_description)/urdf/spot_arm_macro.urdf" />
-            <xacro:load_arm tf_prefix="${tf_prefix}" gripperless="${gripperless}" />
+            <xacro:load_arm tf_prefix="${tf_prefix}"
+                            gripperless="${gripperless}"
+                            custom_gripper_base_link="${custom_gripper_base_link}" />
         </xacro:if>
 
         <!-- Include links at feet if necessary -->

From c638955826cbd4171fba2862f99de29d11180ac0 Mon Sep 17 00:00:00 2001
From: Katie Hughes <khughes-bdai@theaiinstitute.com>
Date: Thu, 31 Oct 2024 10:37:46 -0400
Subject: [PATCH 4/5] don't expose custom_gripper_base_link through main
 spot.urdf.xacro, only through load_spot macro

---
 spot_description/urdf/spot.urdf.xacro  | 3 +--
 spot_description/urdf/spot_macro.xacro | 2 +-
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/spot_description/urdf/spot.urdf.xacro b/spot_description/urdf/spot.urdf.xacro
index 760a49bf1..0380aa69d 100644
--- a/spot_description/urdf/spot.urdf.xacro
+++ b/spot_description/urdf/spot.urdf.xacro
@@ -13,7 +13,6 @@
 
   <!-- Parameters used if Spot has an arm but not a gripper, or is outfitted with a custom gripper. -->
   <xacro:arg name="gripperless" default="false" />
-  <xacro:arg name="custom_gripper_base_link" default="" />
 
   <!-- Parameters for ROS 2 control -->
   <xacro:arg name="add_ros2_control_tag" default="false" />
@@ -30,7 +29,7 @@
     feet="$(arg feet)"
     tf_prefix="$(arg tf_prefix)" 
     gripperless="$(arg gripperless)"
-    custom_gripper_base_link="$(arg custom_gripper_base_link)"/>
+    custom_gripper_base_link=""/>
 
   <!-- Adding the ROS 2 control tags -->
   <xacro:if value="$(arg add_ros2_control_tag)">
diff --git a/spot_description/urdf/spot_macro.xacro b/spot_description/urdf/spot_macro.xacro
index fd36ad15c..f145f1b96 100644
--- a/spot_description/urdf/spot_macro.xacro
+++ b/spot_description/urdf/spot_macro.xacro
@@ -5,7 +5,7 @@
     arm:=false
     feet:=false
     gripperless:=false
-    custom_gripper_base_link
+    custom_gripper_base_link:=''
     tf_prefix">
 
         <link name="${tf_prefix}body">

From 3018d2feb73026540b5f079e46cbbf648d53a545 Mon Sep 17 00:00:00 2001
From: Katie Hughes <khughes-bdai@theaiinstitute.com>
Date: Thu, 31 Oct 2024 10:51:44 -0400
Subject: [PATCH 5/5] add descriptions to parameters

---
 spot_description/urdf/spot.urdf.xacro | 15 ++++++++++-----
 1 file changed, 10 insertions(+), 5 deletions(-)

diff --git a/spot_description/urdf/spot.urdf.xacro b/spot_description/urdf/spot.urdf.xacro
index 0380aa69d..51774251e 100644
--- a/spot_description/urdf/spot.urdf.xacro
+++ b/spot_description/urdf/spot.urdf.xacro
@@ -7,13 +7,19 @@
   <xacro:include filename="$(find spot_description)/urdf/spot.ros2_control.xacro" />
 
   <!-- General parameters -->
-  <xacro:arg name="arm" default="false" />
-  <xacro:arg name="feet" default="false" />
+
+  <!-- String that will be prepended to all joints and links-->
   <xacro:arg name="tf_prefix" default="" />
 
-  <!-- Parameters used if Spot has an arm but not a gripper, or is outfitted with a custom gripper. -->
+  <!-- Set to true if Spot has an arm -->
+  <xacro:arg name="arm" default="false" />
+
+  <!-- Set to true if Spot has an arm but not a gripper -->
   <xacro:arg name="gripperless" default="false" />
 
+  <!-- Set to true to put links at the feet of Spot -->
+  <xacro:arg name="feet" default="false" />
+
   <!-- Parameters for ROS 2 control -->
   <xacro:arg name="add_ros2_control_tag" default="false" />
   <xacro:arg name="hardware_interface_type" default="mock" />
@@ -28,8 +34,7 @@
     arm="$(arg arm)"
     feet="$(arg feet)"
     tf_prefix="$(arg tf_prefix)" 
-    gripperless="$(arg gripperless)"
-    custom_gripper_base_link=""/>
+    gripperless="$(arg gripperless)"/>
 
   <!-- Adding the ROS 2 control tags -->
   <xacro:if value="$(arg add_ros2_control_tag)">