[SW-1266] Add option in spot urdf to put links at the feet (#447)

## Change Overview This now lets you generate a URDF of spot that puts links at the feet locations. It is possible to visualize this with `ros2 launch spot_description description.launch.py feet:=true`. The default URDF that gets generated for running the driver does not include these new frames. ## Testing Done - [x] visualizing the new URDF containing feet frames has them in the right location - [x] default URDF is unchanged ![Screenshot from 2024-08-08 14-55-57.png](https://graphite-user-uploaded-assets-prod.s3.amazonaws.com/Xynj6CBpA3NqqBFfE8Q9/fa798b1a-6ad6-4076-ac86-8554ff4a2ae4.png)
bdaiinstitute · Aug 9, 2024 · d20830c · d20830c
1 parent aab1326
commit d20830c
Show file tree

Hide file tree

Showing 4 changed files with 71 additions and 2 deletions.
diff --git a/spot_description/launch/description.launch.py b/spot_description/launch/description.launch.py
@@ -16,15 +16,29 @@ def generate_launch_description() -> launch.LaunchDescription:
     return launch.LaunchDescription(
         [
             DeclareLaunchArgument(
-                name="gui", default_value="True", description="Flag to enable joint_state_publisher_gui"
+                name="gui",
+                default_value="True",
+                choices=["True", "true", "False", "false"],
+                description="Flag to enable joint_state_publisher_gui",
             ),
             DeclareLaunchArgument(
                 name="model", default_value=default_model_path, description="Absolute path to robot urdf file"
             ),
             DeclareLaunchArgument(
                 name="rvizconfig", default_value=default_rviz2_path, description="Absolute path to rviz config file"
             ),
-            DeclareLaunchArgument(name="arm", default_value="False", description="Flag to enable arm"),
+            DeclareLaunchArgument(
+                name="arm",
+                default_value="False",
+                choices=["True", "true", "False", "false"],
+                description="Flag to enable arm",
+            ),
+            DeclareLaunchArgument(
+                name="feet",
+                default_value="False",
+                choices=["True", "true", "False", "false"],
+                description="Flag to enable putting frames at the feet",
+            ),
             DeclareLaunchArgument(
                 "tf_prefix", default_value='""', description="Apply namespace prefix to robot links and joints"
             ),
@@ -40,6 +54,8 @@ def generate_launch_description() -> launch.LaunchDescription:
                                 LaunchConfiguration("model"),
                                 " arm:=",
                                 LaunchConfiguration("arm"),
+                                " feet:=",
+                                LaunchConfiguration("feet"),
                                 " tf_prefix:=",
                                 LaunchConfiguration("tf_prefix"),
                             ]

diff --git a/spot_description/urdf/spot.urdf.xacro b/spot_description/urdf/spot.urdf.xacro
@@ -5,11 +5,13 @@
 
   <!-- Parameters -->
   <xacro:arg name="arm" default="false" />
+  <xacro:arg name="feet" default="false" />
   <xacro:arg name="tf_prefix" default="" />
 
   <!-- Load Spot -->
   <xacro:load_spot
     arm="$(arg arm)"
+    feet="$(arg feet)"
     tf_prefix="$(arg tf_prefix)" />
 
 </robot>
diff --git a/spot_description/urdf/spot_feet_macro.urdf b/spot_description/urdf/spot_feet_macro.urdf
@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="load_feet" params="tf_prefix">
+
+    <!-- This is the distance from foot to knee in m -->
+    <xacro:property name="foot_to_knee" value="0.35"/>
+
+    <link name="${tf_prefix}front_left_foot"/>
+    <joint name="${tf_prefix}front_left_ankle" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 ${-foot_to_knee}"/>
+      <axis xyz="0 0 0"/>
+      <parent link="${tf_prefix}front_left_lower_leg"/>
+      <child link="${tf_prefix}front_left_foot"/>
+    </joint>
+
+    <link name="${tf_prefix}front_right_foot"/>
+    <joint name="${tf_prefix}front_right_ankle" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 ${-foot_to_knee}"/>
+      <axis xyz="0 0 0"/>
+      <parent link="${tf_prefix}front_right_lower_leg"/>
+      <child link="${tf_prefix}front_right_foot"/>
+    </joint>
+
+    <link name="${tf_prefix}rear_left_foot"/>
+    <joint name="${tf_prefix}rear_left_ankle" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 ${-foot_to_knee}"/>
+      <axis xyz="0 0 0"/>
+      <parent link="${tf_prefix}rear_left_lower_leg"/>
+      <child link="${tf_prefix}rear_left_foot"/>
+    </joint>
+
+    <link name="${tf_prefix}rear_right_foot"/>
+    <joint name="${tf_prefix}rear_right_ankle" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 ${-foot_to_knee}"/>
+      <axis xyz="0 0 0"/>
+      <parent link="${tf_prefix}rear_right_lower_leg"/>
+      <child link="${tf_prefix}rear_right_foot"/>
+    </joint>
+
+  </xacro:macro>
+
+</robot>
diff --git a/spot_description/urdf/spot_macro.xacro b/spot_description/urdf/spot_macro.xacro
@@ -3,6 +3,7 @@
 <robot name="spot" xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:macro name="load_spot" params="
     arm:=false
+    feet:=false
     tf_prefix">
 
         <link name="${tf_prefix}body">
@@ -416,5 +417,11 @@
             <xacro:include filename="$(find spot_description)/urdf/spot_arm_macro.urdf" />
             <xacro:load_arm tf_prefix="${tf_prefix}" />
         </xacro:if>
+
+        <!-- Include links at feet if necessary -->
+        <xacro:if value="${feet}">
+            <xacro:include filename="$(find spot_description)/urdf/spot_feet_macro.urdf" />
+            <xacro:load_feet tf_prefix="${tf_prefix}" />
+        </xacro:if>
     </xacro:macro>
 </robot>