From febcfc05a1f8bc1f4b5a9883cba820221a2ec97e 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] 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 90f58519..760a49bf 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 f7e7aa92..f15882bc 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 1b99c6af..fd36ad15 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 -->