Parameterize ada_moveit with end_effector_tool

ada_description/launch/view_ada.launch.py

@@ -60,23 +60,24 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "description_file",
-            default_value="ada.xacro",
+            default_value="ada_standalone.xacro",
             description="URDF/XACRO description file with the robot.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "use_forque",
-            default_value="false",
-            description="If the forque apparatus is being used.",
+            "end_effector_tool",
+            default_value="none",
+            description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'",
+            choices=["none", "fork", "articulable_fork"],
         )
     )
 
     # Initialize Arguments
     # General arguments
     description_package = LaunchConfiguration("description_package")
     description_file = LaunchConfiguration("description_file")
-    use_forque = LaunchConfiguration("use_forque")
+    end_effector_tool = LaunchConfiguration("end_effector_tool")
 
     robot_description_content = Command(
         [
@@ -86,8 +87,8 @@ def generate_launch_description():
                 [FindPackageShare(description_package), "urdf", description_file]
             ),
             " ",
-            "use_forque:=",
-            use_forque,
+            "end_effector_tool:=",
+            end_effector_tool,
         ]
     )
     robot_description = {

ada_description/rviz/view_robot.rviz

@@ -67,6 +67,30 @@ Visualization Manager:
         Expand Joint Details: false
         Expand Link Details: false
         Expand Tree: false
+        af_link_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        af_link_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        af_motor_fixture_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        af_link_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        af_ft_adapter_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         FT:
           Alpha: 1
           Show Axes: false

ada_description/urdf/ada.xacro

@@ -12,65 +12,63 @@
   xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
     xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
     xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics"
-  xmlns:xacro="http://www.ros.org/wiki/xacro" name="ada">
+  xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:include filename="$(find ada_description)/urdf/forque/forque.xacro"/>
-  <xacro:include filename="$(find ada_description)/urdf/camera/camera.xacro"/>
-  <xacro:include filename="$(find ada_description)/urdf/j2n6s200.xacro"/>
+  <xacro:macro name="ada" params="base_parent end_effector_tool">
+    <xacro:include filename="$(find ada_description)/urdf/forque/forque.xacro"/>
+    <xacro:include filename="$(find ada_description)/urdf/camera/camera.xacro"/>
+    <xacro:include filename="$(find ada_description)/urdf/j2n6s200.xacro"/>
 
-  <xacro:arg name="use_forque" default="true"/>
-
-  <link name="root"/>
-
-  <link name="root_tilt"/>
-  <joint name="robot_tilt" type="revolute">
-    <origin xyz="0 0 0" rpy="0 0 0" />
-    <child link="root_tilt" />
-    <parent link="root" />
-    <axis xyz="-1 0 0"/>
-    <!-- effort copied from j2n6s200 joint 2, velocity is max velocity the wheelchair can tilt,
-    lower and upper limits generously encompass the range of the wheelchair's tilt-->
-    <limit effort="80" velocity="0.15" lower="${-J_PI}" upper="${J_PI}"/>
-    <dynamics damping="0.0" friction="0.0"/>
-  </joint>
+    <link name="root_tilt"/>
+    <joint name="robot_tilt" type="revolute">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <child link="root_tilt" />
+      <parent link="${base_parent}" />
+      <axis xyz="-1 0 0"/>
+      <!-- effort copied from j2n6s200 joint 2, velocity is max velocity the wheelchair can tilt,
+      lower and upper limits generously encompass the range of the wheelchair's tilt-->
+      <limit effort="80" velocity="0.15" lower="${-J_PI}" upper="${J_PI}"/>
+      <dynamics damping="0.0" friction="0.0"/>
+    </joint>
 
-  <xacro:property name="robot_root" value="root_tilt" />
+    <xacro:property name="robot_root" value="root_tilt" />
 
-  <xacro:j2n6s200 base_parent="${robot_root}"/>
+    <xacro:j2n6s200 base_parent="${robot_root}"/>
 
-  <xacro:camera_assembly base_parent="j2n6s200_link_6" base_xyz="0.0165 0 0.0011" base_rpy="${3*J_PI/2} 0 ${3*J_PI/2}"/>
+    <xacro:camera_assembly base_parent="j2n6s200_link_6" base_xyz="0.0165 0 0.0011" base_rpy="${3*J_PI/2} 0 ${3*J_PI/2}"/>
 
-  <!-- Hard-coded Extrinsics -->
-  <link name="camera_link" />
-  <joint name="extrinsics" type="fixed">
-    <origin xyz="0.0194 -0.01606 0.029014" rpy="0.17234 -1.56127 2.96967" />
-    <parent link="cameraMount" />
-    <child link="camera_link" />
-  </joint>
-  <!-- Copy Realsense in case of simulation -->
-  <!--
-  <link name="camera_color_optical_frame" />
-  <joint name="optical_republish" type="fixed">
-    <origin xyz="0.000 -0.015 0.000" rpy="-1.564 0.000 -1.573" />
-    <parent link="camera_link" />
-    <child link="camera_color_optical_frame" />
-  </joint>
-  -->
+    <!-- Hard-coded Extrinsics -->
+    <link name="camera_link" />
+    <joint name="extrinsics" type="fixed">
+      <origin xyz="0.0194 -0.01606 0.029014" rpy="0.17234 -1.56127 2.96967" />
+      <parent link="cameraMount" />
+      <child link="camera_link" />
+    </joint>
+    <!-- Copy Realsense in case of simulation -->
+    <!--
+    <link name="camera_color_optical_frame" />
+    <joint name="optical_republish" type="fixed">
+      <origin xyz="0.000 -0.015 0.000" rpy="-1.564 0.000 -1.573" />
+      <parent link="camera_link" />
+      <child link="camera_color_optical_frame" />
+    </joint>
+    -->
 
-  <xacro:forque_assembly_link link_name="FTArmMount" link_mesh="2024_01_18_FTArmMount" mass="0.03783" cog="0.010957 -0.019223 -0.03777">
-    <inertia ixx="0.000018817" iyy="0.00001119989" iyz="-0.00000120296" izz="0.000026883" ixy="-0.0000003327" ixz="-0.000000092476"/>
-  </xacro:forque_assembly_link>
-  <xacro:forque_assembly_joint joint_name="FTArmMount_to_arm" parent="j2n6s200_link_6" child="FTArmMount" joint_origin_xyz="0.0065 -0.011 -0.0075" joint_origin_rpy="${J_PI/2} 0 ${J_PI/2}"/>
+    <xacro:forque_assembly_link link_name="FTArmMount" link_mesh="2024_01_18_FTArmMount" mass="0.03783" cog="0.010957 -0.019223 -0.03777">
+      <inertia ixx="0.000018817" iyy="0.00001119989" iyz="-0.00000120296" izz="0.000026883" ixy="-0.0000003327" ixz="-0.000000092476"/>
+    </xacro:forque_assembly_link>
+    <xacro:forque_assembly_joint joint_name="FTArmMount_to_arm" type="fixed" parent="j2n6s200_link_6" child="FTArmMount" joint_axis_xyz="0 0 1" joint_origin_xyz="0.0065 -0.011 -0.0075" joint_origin_rpy="${J_PI/2} 0 ${J_PI/2}" fixed="true"/>
 
-  <xacro:if value="$(arg use_forque)">
-    <xacro:forque_assembly base_parent="FTArmMount"/>
+    <xacro:unless value="${end_effector_tool == 'none'}">
+      <xacro:forque_assembly base_parent="FTArmMount" end_effector_tool="${end_effector_tool}"/>
 
-    <link name="FTSensor" />
-    <joint name="EE_to_FTSensor" type="fixed">
-        <child link="FTSensor"/>
-        <parent link="j2n6s200_end_effector"/>
-        <origin rpy="0 0 0" xyz="0.0 0.0 0.092"/>
-    </joint>
-  </xacro:if>
+      <link name="FTSensor" />
+      <joint name="EE_to_FTSensor" type="fixed">
+          <child link="FTSensor"/>
+          <parent link="j2n6s200_end_effector"/>
+          <origin rpy="0 0 0" xyz="0.0 0.0 0.092"/>
+      </joint>
+    </xacro:unless>
+  </xacro:macro>
 
 </robot>

ada_description/urdf/ada_standalone.xacro

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<robot xmlns:xi="http://www.w3.org/2001/XInclude"
+  xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+  xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+  xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+  xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+  xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+  xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+  xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+  xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+  xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+  xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+  xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics"
+  xmlns:xacro="http://www.ros.org/wiki/xacro"
+  name="ada">
+
+  <xacro:arg name="end_effector_tool" default="fork" />
+
+  <xacro:include filename="$(find ada_description)/urdf/ada.xacro" />
+
+  <link name="root"/>
+  <xacro:property name="root" value="root" />
+  <xacro:property name="end_effector_tool" value="$(arg end_effector_tool)" />
+  <xacro:ada base_parent="${root}" end_effector_tool="${end_effector_tool}" />
+
+</robot>