From a7158906bc34256e1982e34ae7adfdb330440043 Mon Sep 17 00:00:00 2001
From: Katie Hughes <khughes-bdai@theaiinstitute.com>
Date: Thu, 20 Feb 2025 15:38:09 -0500
Subject: [PATCH 1/8] Add hardcoded stuff to URDF

---
 spot_description/urdf/spot.ros2_control.xacro | 31 +++++++++++--------
 1 file changed, 18 insertions(+), 13 deletions(-)

diff --git a/spot_description/urdf/spot.ros2_control.xacro b/spot_description/urdf/spot.ros2_control.xacro
index bff9d7f..56db98f 100644
--- a/spot_description/urdf/spot.ros2_control.xacro
+++ b/spot_description/urdf/spot.ros2_control.xacro
@@ -2,7 +2,7 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
     <xacro:macro name="ros2_control_joint"
-        params="joint_name position_min:=0.0 position_max:=0.0 velocity_max effort_max">
+        params="joint_name position_min:=0.0 position_max:=0.0 velocity_max effort_max k_q_p k_qd_p">
         <joint name="${joint_name}">
             <state_interface name="position" />
             <state_interface name="velocity" />
@@ -20,44 +20,49 @@
                 <param name="min">${-effort_max}</param>
                 <param name="max">${effort_max}</param>
             </command_interface>
+            <command_interface name="k_q_p">
+                <param name="initial_value">${k_q_p}</param>
+            </command_interface>
+            <command_interface name="k_qd_p">
+                <param name="initial_value">${k_qd_p}</param>
+            </command_interface>
         </joint>
     </xacro:macro>
 
     <!-- TODO We do not know the real velocity bounds for Spot. -->
     <xacro:macro name="leg" params="left_or_right front_or_rear tf_prefix">
         <xacro:ros2_control_joint joint_name="${tf_prefix}${front_or_rear}_${left_or_right}_hip_x"
-            position_min="-0.785" position_max="0.785" velocity_max="100" effort_max="45" />
+            position_min="-0.785" position_max="0.785" velocity_max="100" effort_max="45" k_q_p="624.0" k_qd_p="5.20" />
 
         <xacro:ros2_control_joint joint_name="${tf_prefix}${front_or_rear}_${left_or_right}_hip_y"
-            position_min="-0.899" position_max="2.295" velocity_max="100" effort_max="45" />
+            position_min="-0.899" position_max="2.295" velocity_max="100" effort_max="45" k_q_p="936.0" k_qd_p="5.20" />
 
         <xacro:ros2_control_joint joint_name="${tf_prefix}${front_or_rear}_${left_or_right}_knee"
-            position_min="-2.793" position_max="-0.255" velocity_max="100" effort_max="115" />
+            position_min="-2.793" position_max="-0.255" velocity_max="100" effort_max="115" k_q_p="286.0" k_qd_p="2.04" />
     </xacro:macro>
 
     <xacro:macro name="arm" params="tf_prefix">
         <xacro:ros2_control_joint joint_name="${tf_prefix}arm_sh0"
-            position_min="-2.168" position_max="3.142" velocity_max="100" effort_max="90.9" />
+            position_min="-2.168" position_max="3.142" velocity_max="100" effort_max="90.9" k_q_p="1020.0" k_qd_p="10.2" />
 
         <xacro:ros2_control_joint joint_name="${tf_prefix}arm_sh1"
-            position_min="-3.142" position_max="0.524" velocity_max="100" effort_max="181.8" />
+            position_min="-3.142" position_max="0.524" velocity_max="100" effort_max="181.8" k_q_p="255.0" k_qd_p="15.3" />
 
         <xacro:ros2_control_joint joint_name="${tf_prefix}arm_el0"
-            position_min="0.000" position_max="3.142" velocity_max="100" effort_max="90.9" />
+            position_min="0.000" position_max="3.142" velocity_max="100" effort_max="90.9" k_q_p="204.0" k_qd_p="10.2" />
 
         <xacro:ros2_control_joint joint_name="${tf_prefix}arm_el1"
-            position_min="-2.793" position_max="2.793" velocity_max="100" effort_max="30.3" />
+            position_min="-2.793" position_max="2.793" velocity_max="100" effort_max="30.3" k_q_p="102.0" k_qd_p="2.04" />
 
         <xacro:ros2_control_joint joint_name="${tf_prefix}arm_wr0"
-            position_min="-1.833" position_max="1.833" velocity_max="100" effort_max="30.3" />
+            position_min="-1.833" position_max="1.833" velocity_max="100" effort_max="30.3" k_q_p="102.0" k_qd_p="2.04" />
 
         <xacro:ros2_control_joint joint_name="${tf_prefix}arm_wr1"
-            position_min="-2.880" position_max="2.880" velocity_max="100" effort_max="30.3" />
+            position_min="-2.880" position_max="2.880" velocity_max="100" effort_max="30.3" k_q_p="102.0" k_qd_p="2.04" />
 
-        <!-- This is the gripper joint. This could be its own interface to support robots
-        with custom grippers, but would involve modifying the URDFs in spot_description. -->
+        <!-- Note: this would have to be modified if we want to run as gripperless -->
         <xacro:ros2_control_joint joint_name="${tf_prefix}arm_f1x"
-            position_min="-1.570" position_max="0.000" velocity_max="100" effort_max="15.3" />
+            position_min="-1.570" position_max="0.000" velocity_max="100" effort_max="15.3" k_q_p="16.0" k_qd_p="0.32" />
 
     </xacro:macro>
 

From f8ca213e470d4f9a0e2e8f0a155bddfd41d0b2ef Mon Sep 17 00:00:00 2001
From: Katie Hughes <khughes-bdai@theaiinstitute.com>
Date: Mon, 24 Feb 2025 11:33:06 -0500
Subject: [PATCH 2/8] some incredibly ugly xacro logic, but i think it's
 funcitonal

---
 spot_description/urdf/spot.ros2_control.xacro | 59 ++++++++++++-------
 1 file changed, 38 insertions(+), 21 deletions(-)

diff --git a/spot_description/urdf/spot.ros2_control.xacro b/spot_description/urdf/spot.ros2_control.xacro
index 56db98f..f4029eb 100644
--- a/spot_description/urdf/spot.ros2_control.xacro
+++ b/spot_description/urdf/spot.ros2_control.xacro
@@ -30,45 +30,46 @@
     </xacro:macro>
 
     <!-- TODO We do not know the real velocity bounds for Spot. -->
-    <xacro:macro name="leg" params="left_or_right front_or_rear tf_prefix">
+    <xacro:macro name="leg" params="left_or_right front_or_rear tf_prefix k_q_p_list k_qd_p_list">
+        <!-- it's assumed that k_q_p_list and k_qd_list have 3 elements in them, where index 0 corresponds to hip x, 1 to hip y, and 2 to knee.-->
         <xacro:ros2_control_joint joint_name="${tf_prefix}${front_or_rear}_${left_or_right}_hip_x"
-            position_min="-0.785" position_max="0.785" velocity_max="100" effort_max="45" k_q_p="624.0" k_qd_p="5.20" />
+            position_min="-0.785" position_max="0.785" velocity_max="100" effort_max="45" k_q_p="${k_q_p_list[0]}" k_qd_p="${k_qd_p_list[0]}" />
 
         <xacro:ros2_control_joint joint_name="${tf_prefix}${front_or_rear}_${left_or_right}_hip_y"
-            position_min="-0.899" position_max="2.295" velocity_max="100" effort_max="45" k_q_p="936.0" k_qd_p="5.20" />
+            position_min="-0.899" position_max="2.295" velocity_max="100" effort_max="45" k_q_p="${k_q_p_list[1]}" k_qd_p="${k_qd_p_list[1]}" />
 
         <xacro:ros2_control_joint joint_name="${tf_prefix}${front_or_rear}_${left_or_right}_knee"
-            position_min="-2.793" position_max="-0.255" velocity_max="100" effort_max="115" k_q_p="286.0" k_qd_p="2.04" />
+            position_min="-2.793" position_max="-0.255" velocity_max="100" effort_max="115" k_q_p="${k_q_p_list[2]}" k_qd_p="${k_qd_p_list[2]}" />
     </xacro:macro>
 
-    <xacro:macro name="arm" params="tf_prefix">
+    <xacro:macro name="arm" params="tf_prefix k_q_p_list k_qd_p_list">
+        <!-- it's assumed that k_q_p_list and k_qd_list have 7 elements in them, going in order from shoulder to finger.-->
         <xacro:ros2_control_joint joint_name="${tf_prefix}arm_sh0"
-            position_min="-2.168" position_max="3.142" velocity_max="100" effort_max="90.9" k_q_p="1020.0" k_qd_p="10.2" />
+            position_min="-2.168" position_max="3.142" velocity_max="100" effort_max="90.9" k_q_p="${k_q_p_list[0]}" k_qd_p="${k_qd_p_list[0]}" />
 
         <xacro:ros2_control_joint joint_name="${tf_prefix}arm_sh1"
-            position_min="-3.142" position_max="0.524" velocity_max="100" effort_max="181.8" k_q_p="255.0" k_qd_p="15.3" />
+            position_min="-3.142" position_max="0.524" velocity_max="100" effort_max="181.8" k_q_p="${k_q_p_list[1]}" k_qd_p="${k_qd_p_list[1]}" />
 
         <xacro:ros2_control_joint joint_name="${tf_prefix}arm_el0"
-            position_min="0.000" position_max="3.142" velocity_max="100" effort_max="90.9" k_q_p="204.0" k_qd_p="10.2" />
+            position_min="0.000" position_max="3.142" velocity_max="100" effort_max="90.9" k_q_p="${k_q_p_list[2]}" k_qd_p="${k_qd_p_list[2]}" />
 
         <xacro:ros2_control_joint joint_name="${tf_prefix}arm_el1"
-            position_min="-2.793" position_max="2.793" velocity_max="100" effort_max="30.3" k_q_p="102.0" k_qd_p="2.04" />
+            position_min="-2.793" position_max="2.793" velocity_max="100" effort_max="30.3" k_q_p="${k_q_p_list[3]}" k_qd_p="${k_qd_p_list[3]}" />
 
         <xacro:ros2_control_joint joint_name="${tf_prefix}arm_wr0"
-            position_min="-1.833" position_max="1.833" velocity_max="100" effort_max="30.3" k_q_p="102.0" k_qd_p="2.04" />
+            position_min="-1.833" position_max="1.833" velocity_max="100" effort_max="30.3" k_q_p="${k_q_p_list[4]}" k_qd_p="${k_qd_p_list[4]}" />
 
         <xacro:ros2_control_joint joint_name="${tf_prefix}arm_wr1"
-            position_min="-2.880" position_max="2.880" velocity_max="100" effort_max="30.3" k_q_p="102.0" k_qd_p="2.04" />
+            position_min="-2.880" position_max="2.880" velocity_max="100" effort_max="30.3" k_q_p="${k_q_p_list[5]}" k_qd_p="${k_qd_p_list[5]}" />
 
         <!-- Note: this would have to be modified if we want to run as gripperless -->
         <xacro:ros2_control_joint joint_name="${tf_prefix}arm_f1x"
-            position_min="-1.570" position_max="0.000" velocity_max="100" effort_max="15.3" k_q_p="16.0" k_qd_p="0.32" />
+            position_min="-1.570" position_max="0.000" velocity_max="100" effort_max="15.3" k_q_p="${k_q_p_list[6]}" k_qd_p="${k_qd_p_list[6]}" />
 
     </xacro:macro>
 
     <xacro:macro name="spot_ros2_control" params="interface_type has_arm leasing hostname port certificate username password tf_prefix k_q_p k_qd_p">
-        <!-- Currently implements a simple system interface that covers all joints of the robot.
-        In the future, we could make different hardware interfaces for the body, arm, etc. -->
+        <!-- Currently implements a simple system interface that covers all joints of the robot -->
         <ros2_control name="SpotSystem" type="system">
             <hardware>
             <xacro:if value="${interface_type == 'mock'}">
@@ -88,14 +89,30 @@
                 <param name="k_qd_p">${k_qd_p}</param>
             </xacro:if>
             </hardware>
-            <!-- Add the legs -->
-            <xacro:leg front_or_rear="front" left_or_right="left" tf_prefix="${tf_prefix}" />
-            <xacro:leg front_or_rear="front" left_or_right="right" tf_prefix="${tf_prefix}" />
-            <xacro:leg front_or_rear="rear" left_or_right="left" tf_prefix="${tf_prefix}" />
-            <xacro:leg front_or_rear="rear" left_or_right="right" tf_prefix="${tf_prefix}" />
-            <!-- Add the arm + the gripper if the robot has an arm -->
+
+            <!-- parse the sting seperated input value into a list for easier operations. -->
+            <xacro:property name="k_q_p_arg" value="${[float(i) for i in k_q_p.split()]}"/>
+            <xacro:property name="k_qd_p_arg" value="${[float(i) for i in k_qd_p.split()]}"/>
+            <xacro:property name="default_k_q_p" value="${[624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 1020.0, 255.0, 204.0, 102.0, 102.0, 102.0, 16.0]}"/>
+            <xacro:property name="default_k_qd_p" value="${[5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 10.2, 15.3, 10.2, 2.04, 2.04, 2.04, 0.32]}"/>
+
+            <!-- number of expected elements in the gain string depending on if the robot has an arm. -->
+            <xacro:property name="njoints" value="${19 if has_arm else 12}"/>
+
+            <!-- ensure that the values we are working with have the appropriate number of joints.
+            If the user passed string is correctly formatted, use that, else fall back to the default values. -->
+            <xacro:property name="k_q_p_used" value="${k_q_p_arg if len(k_q_p_arg) == njoints else default_k_q_p[:njoints]}"/>
+            <xacro:property name="k_qd_p_used" value="${k_qd_p_arg if len(k_qd_p_arg) == njoints else default_k_qd_p[:njoints]}"/>
+
+            <!-- Add the legs, setting the initial gain values to the appropriate values. -->
+            <xacro:leg front_or_rear="front" left_or_right="left"  tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[:3]}"   k_qd_p_list="${k_qd_p_used[:3]}"/>
+            <xacro:leg front_or_rear="front" left_or_right="right" tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[3:6]}"  k_qd_p_list="${k_qd_p_used[3:6]}"/>
+            <xacro:leg front_or_rear="rear" left_or_right="left"   tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[6:9]}"  k_qd_p_list="${k_qd_p_used[6:9]}"/>
+            <xacro:leg front_or_rear="rear" left_or_right="right"  tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[9:12]}" k_qd_p_list="${k_qd_p_used[9:12]}"/>
+
+            <!-- Add the arm + the gripper if the robot has an arm, again setting the initial gain values to the appropriate values.  -->
             <xacro:if value="${has_arm}">
-                <xacro:arm tf_prefix="${tf_prefix}" />
+                <xacro:arm tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[12:]}" k_qd_p_list="${k_qd_p_used[12:]}"/>
             </xacro:if>
         </ros2_control>
     </xacro:macro>

From b39eaefc1cd8729ab2cedaa0fac2120fe2a30d49 Mon Sep 17 00:00:00 2001
From: Katie Hughes <khughes-bdai@theaiinstitute.com>
Date: Mon, 24 Feb 2025 11:33:57 -0500
Subject: [PATCH 3/8] Comment better

---
 spot_description/urdf/spot.ros2_control.xacro | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/spot_description/urdf/spot.ros2_control.xacro b/spot_description/urdf/spot.ros2_control.xacro
index f4029eb..b2804d5 100644
--- a/spot_description/urdf/spot.ros2_control.xacro
+++ b/spot_description/urdf/spot.ros2_control.xacro
@@ -93,12 +93,11 @@
             <!-- parse the sting seperated input value into a list for easier operations. -->
             <xacro:property name="k_q_p_arg" value="${[float(i) for i in k_q_p.split()]}"/>
             <xacro:property name="k_qd_p_arg" value="${[float(i) for i in k_qd_p.split()]}"/>
+            <!-- default gain values provided by BD -->
             <xacro:property name="default_k_q_p" value="${[624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 1020.0, 255.0, 204.0, 102.0, 102.0, 102.0, 16.0]}"/>
             <xacro:property name="default_k_qd_p" value="${[5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 10.2, 15.3, 10.2, 2.04, 2.04, 2.04, 0.32]}"/>
-
             <!-- number of expected elements in the gain string depending on if the robot has an arm. -->
             <xacro:property name="njoints" value="${19 if has_arm else 12}"/>
-
             <!-- ensure that the values we are working with have the appropriate number of joints.
             If the user passed string is correctly formatted, use that, else fall back to the default values. -->
             <xacro:property name="k_q_p_used" value="${k_q_p_arg if len(k_q_p_arg) == njoints else default_k_q_p[:njoints]}"/>

From cdb8607fd3b48a7651b12b16a9b506e6d22a12b2 Mon Sep 17 00:00:00 2001
From: Katie Hughes <khughes-bdai@theaiinstitute.com>
Date: Mon, 24 Feb 2025 12:50:03 -0500
Subject: [PATCH 4/8] nit

---
 spot_description/urdf/spot.ros2_control.xacro | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/spot_description/urdf/spot.ros2_control.xacro b/spot_description/urdf/spot.ros2_control.xacro
index b2804d5..9cba9e6 100644
--- a/spot_description/urdf/spot.ros2_control.xacro
+++ b/spot_description/urdf/spot.ros2_control.xacro
@@ -104,10 +104,10 @@
             <xacro:property name="k_qd_p_used" value="${k_qd_p_arg if len(k_qd_p_arg) == njoints else default_k_qd_p[:njoints]}"/>
 
             <!-- Add the legs, setting the initial gain values to the appropriate values. -->
-            <xacro:leg front_or_rear="front" left_or_right="left"  tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[:3]}"   k_qd_p_list="${k_qd_p_used[:3]}"/>
-            <xacro:leg front_or_rear="front" left_or_right="right" tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[3:6]}"  k_qd_p_list="${k_qd_p_used[3:6]}"/>
-            <xacro:leg front_or_rear="rear" left_or_right="left"   tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[6:9]}"  k_qd_p_list="${k_qd_p_used[6:9]}"/>
-            <xacro:leg front_or_rear="rear" left_or_right="right"  tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[9:12]}" k_qd_p_list="${k_qd_p_used[9:12]}"/>
+            <xacro:leg front_or_rear="front" left_or_right="left"  tf_prefix="${tf_prefix}"  k_q_p_list="${k_q_p_used[:3]}"   k_qd_p_list="${k_qd_p_used[:3]}"/>
+            <xacro:leg front_or_rear="front" left_or_right="right" tf_prefix="${tf_prefix}"  k_q_p_list="${k_q_p_used[3:6]}"  k_qd_p_list="${k_qd_p_used[3:6]}"/>
+            <xacro:leg front_or_rear="rear"  left_or_right="left"   tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[6:9]}"  k_qd_p_list="${k_qd_p_used[6:9]}"/>
+            <xacro:leg front_or_rear="rear"  left_or_right="right"  tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[9:12]}" k_qd_p_list="${k_qd_p_used[9:12]}"/>
 
             <!-- Add the arm + the gripper if the robot has an arm, again setting the initial gain values to the appropriate values.  -->
             <xacro:if value="${has_arm}">

From e7460d638cf9fac5f92c4c7554f7a513c572095e Mon Sep 17 00:00:00 2001
From: Katie Hughes <khughes-bdai@theaiinstitute.com>
Date: Mon, 24 Feb 2025 12:51:59 -0500
Subject: [PATCH 5/8] nit

---
 spot_description/urdf/spot.ros2_control.xacro | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/spot_description/urdf/spot.ros2_control.xacro b/spot_description/urdf/spot.ros2_control.xacro
index 9cba9e6..d9fa82b 100644
--- a/spot_description/urdf/spot.ros2_control.xacro
+++ b/spot_description/urdf/spot.ros2_control.xacro
@@ -104,10 +104,10 @@
             <xacro:property name="k_qd_p_used" value="${k_qd_p_arg if len(k_qd_p_arg) == njoints else default_k_qd_p[:njoints]}"/>
 
             <!-- Add the legs, setting the initial gain values to the appropriate values. -->
-            <xacro:leg front_or_rear="front" left_or_right="left"  tf_prefix="${tf_prefix}"  k_q_p_list="${k_q_p_used[:3]}"   k_qd_p_list="${k_qd_p_used[:3]}"/>
-            <xacro:leg front_or_rear="front" left_or_right="right" tf_prefix="${tf_prefix}"  k_q_p_list="${k_q_p_used[3:6]}"  k_qd_p_list="${k_qd_p_used[3:6]}"/>
-            <xacro:leg front_or_rear="rear"  left_or_right="left"   tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[6:9]}"  k_qd_p_list="${k_qd_p_used[6:9]}"/>
-            <xacro:leg front_or_rear="rear"  left_or_right="right"  tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[9:12]}" k_qd_p_list="${k_qd_p_used[9:12]}"/>
+            <xacro:leg front_or_rear="front" left_or_right="left"  tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[:3]}"   k_qd_p_list="${k_qd_p_used[:3]}"/>
+            <xacro:leg front_or_rear="front" left_or_right="right" tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[3:6]}"  k_qd_p_list="${k_qd_p_used[3:6]}"/>
+            <xacro:leg front_or_rear="rear"  left_or_right="left"  tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[6:9]}"  k_qd_p_list="${k_qd_p_used[6:9]}"/>
+            <xacro:leg front_or_rear="rear"  left_or_right="right" tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[9:12]}" k_qd_p_list="${k_qd_p_used[9:12]}"/>
 
             <!-- Add the arm + the gripper if the robot has an arm, again setting the initial gain values to the appropriate values.  -->
             <xacro:if value="${has_arm}">

From 7b600c82f657f39c3aca4b53c2653b6086d4911c Mon Sep 17 00:00:00 2001
From: Katie Hughes <khughes-bdai@theaiinstitute.com>
Date: Mon, 24 Feb 2025 12:52:25 -0500
Subject: [PATCH 6/8] remove gains from SpotHardware args

---
 spot_description/urdf/spot.ros2_control.xacro | 2 --
 1 file changed, 2 deletions(-)

diff --git a/spot_description/urdf/spot.ros2_control.xacro b/spot_description/urdf/spot.ros2_control.xacro
index d9fa82b..90e3749 100644
--- a/spot_description/urdf/spot.ros2_control.xacro
+++ b/spot_description/urdf/spot.ros2_control.xacro
@@ -85,8 +85,6 @@
                 <param name="username">$(optenv BOSDYN_CLIENT_USERNAME ${username})</param>
                 <param name="password">$(optenv BOSDYN_CLIENT_PASSWORD ${password})</param>
                 <param name="leasing">${leasing}</param>
-                <param name="k_q_p">${k_q_p}</param>
-                <param name="k_qd_p">${k_qd_p}</param>
             </xacro:if>
             </hardware>
 

From 568aaf9cf32a4a57c9158aca68dbd3e5563ce019 Mon Sep 17 00:00:00 2001
From: Katie Hughes <khughes-bdai@theaiinstitute.com>
Date: Mon, 24 Feb 2025 12:55:25 -0500
Subject: [PATCH 7/8] add link for reference to default gains

---
 spot_description/urdf/spot.ros2_control.xacro | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/spot_description/urdf/spot.ros2_control.xacro b/spot_description/urdf/spot.ros2_control.xacro
index 90e3749..e4a2cd5 100644
--- a/spot_description/urdf/spot.ros2_control.xacro
+++ b/spot_description/urdf/spot.ros2_control.xacro
@@ -91,7 +91,8 @@
             <!-- parse the sting seperated input value into a list for easier operations. -->
             <xacro:property name="k_q_p_arg" value="${[float(i) for i in k_q_p.split()]}"/>
             <xacro:property name="k_qd_p_arg" value="${[float(i) for i in k_qd_p.split()]}"/>
-            <!-- default gain values provided by BD -->
+            <!-- default gain values provided by BD:
+            https://github.com/boston-dynamics/spot-cpp-sdk/blob/master/cpp/examples/joint_control/constants.hpp -->
             <xacro:property name="default_k_q_p" value="${[624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 1020.0, 255.0, 204.0, 102.0, 102.0, 102.0, 16.0]}"/>
             <xacro:property name="default_k_qd_p" value="${[5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 10.2, 15.3, 10.2, 2.04, 2.04, 2.04, 0.32]}"/>
             <!-- number of expected elements in the gain string depending on if the robot has an arm. -->

From fd9eb3a0f7358f35c07d4be79a98d3fa812e1e2d Mon Sep 17 00:00:00 2001
From: Katie Hughes <khughes-bdai@theaiinstitute.com>
Date: Mon, 24 Feb 2025 13:59:11 -0500
Subject: [PATCH 8/8] Slightly unify on names

---
 spot_description/urdf/spot.ros2_control.xacro | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/spot_description/urdf/spot.ros2_control.xacro b/spot_description/urdf/spot.ros2_control.xacro
index e4a2cd5..3a1981b 100644
--- a/spot_description/urdf/spot.ros2_control.xacro
+++ b/spot_description/urdf/spot.ros2_control.xacro
@@ -93,24 +93,24 @@
             <xacro:property name="k_qd_p_arg" value="${[float(i) for i in k_qd_p.split()]}"/>
             <!-- default gain values provided by BD:
             https://github.com/boston-dynamics/spot-cpp-sdk/blob/master/cpp/examples/joint_control/constants.hpp -->
-            <xacro:property name="default_k_q_p" value="${[624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 1020.0, 255.0, 204.0, 102.0, 102.0, 102.0, 16.0]}"/>
-            <xacro:property name="default_k_qd_p" value="${[5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 10.2, 15.3, 10.2, 2.04, 2.04, 2.04, 0.32]}"/>
+            <xacro:property name="k_q_p_default" value="${[624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 1020.0, 255.0, 204.0, 102.0, 102.0, 102.0, 16.0]}"/>
+            <xacro:property name="k_qd_p_default" value="${[5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 10.2, 15.3, 10.2, 2.04, 2.04, 2.04, 0.32]}"/>
             <!-- number of expected elements in the gain string depending on if the robot has an arm. -->
             <xacro:property name="njoints" value="${19 if has_arm else 12}"/>
             <!-- ensure that the values we are working with have the appropriate number of joints.
             If the user passed string is correctly formatted, use that, else fall back to the default values. -->
-            <xacro:property name="k_q_p_used" value="${k_q_p_arg if len(k_q_p_arg) == njoints else default_k_q_p[:njoints]}"/>
-            <xacro:property name="k_qd_p_used" value="${k_qd_p_arg if len(k_qd_p_arg) == njoints else default_k_qd_p[:njoints]}"/>
+            <xacro:property name="k_q_p_validated" value="${k_q_p_arg if len(k_q_p_arg) == njoints else k_q_p_default[:njoints]}"/>
+            <xacro:property name="k_qd_p_validated" value="${k_qd_p_arg if len(k_qd_p_arg) == njoints else k_qd_p_default[:njoints]}"/>
 
             <!-- Add the legs, setting the initial gain values to the appropriate values. -->
-            <xacro:leg front_or_rear="front" left_or_right="left"  tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[:3]}"   k_qd_p_list="${k_qd_p_used[:3]}"/>
-            <xacro:leg front_or_rear="front" left_or_right="right" tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[3:6]}"  k_qd_p_list="${k_qd_p_used[3:6]}"/>
-            <xacro:leg front_or_rear="rear"  left_or_right="left"  tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[6:9]}"  k_qd_p_list="${k_qd_p_used[6:9]}"/>
-            <xacro:leg front_or_rear="rear"  left_or_right="right" tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[9:12]}" k_qd_p_list="${k_qd_p_used[9:12]}"/>
+            <xacro:leg front_or_rear="front" left_or_right="left"  tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_validated[:3]}"   k_qd_p_list="${k_qd_p_validated[:3]}"/>
+            <xacro:leg front_or_rear="front" left_or_right="right" tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_validated[3:6]}"  k_qd_p_list="${k_qd_p_validated[3:6]}"/>
+            <xacro:leg front_or_rear="rear"  left_or_right="left"  tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_validated[6:9]}"  k_qd_p_list="${k_qd_p_validated[6:9]}"/>
+            <xacro:leg front_or_rear="rear"  left_or_right="right" tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_validated[9:12]}" k_qd_p_list="${k_qd_p_validated[9:12]}"/>
 
             <!-- Add the arm + the gripper if the robot has an arm, again setting the initial gain values to the appropriate values.  -->
             <xacro:if value="${has_arm}">
-                <xacro:arm tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_used[12:]}" k_qd_p_list="${k_qd_p_used[12:]}"/>
+                <xacro:arm tf_prefix="${tf_prefix}" k_q_p_list="${k_q_p_validated[12:]}" k_qd_p_list="${k_qd_p_validated[12:]}"/>
             </xacro:if>
         </ros2_control>
     </xacro:macro>