Configurable arm_id (moveit#106)

config/arm.xacro

@@ -7,67 +7,67 @@
         <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
         <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
         <group name="${name}">
-            <chain base_link="panda_link0" tip_link="${tip_link}" />
+            <chain base_link="$(arg arm_id)_link0" tip_link="${tip_link}" />
         </group>
         <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
         <group_state name="ready" group="${name}">
-            <joint name="panda_joint1" value="0" />
-            <joint name="panda_joint2" value="${-pi/4}" />
-            <joint name="panda_joint3" value="0" />
-            <joint name="panda_joint4" value="${-3*pi/4}" />
-            <joint name="panda_joint5" value="0" />
-            <joint name="panda_joint6" value="${pi/2}" />
-            <joint name="panda_joint7" value="${pi/4}" />
+            <joint name="$(arg arm_id)_joint1" value="0" />
+            <joint name="$(arg arm_id)_joint2" value="${-pi/4}" />
+            <joint name="$(arg arm_id)_joint3" value="0" />
+            <joint name="$(arg arm_id)_joint4" value="${-3*pi/4}" />
+            <joint name="$(arg arm_id)_joint5" value="0" />
+            <joint name="$(arg arm_id)_joint6" value="${pi/2}" />
+            <joint name="$(arg arm_id)_joint7" value="${pi/4}" />
         </group_state>
         <group_state name="extended" group="${name}">
-            <joint name="panda_joint1" value="0" />
-            <joint name="panda_joint2" value="0" />
-            <joint name="panda_joint3" value="0" />
-            <joint name="panda_joint4" value="-0.1" />
-            <joint name="panda_joint5" value="0" />
-            <joint name="panda_joint6" value="${pi}" />
-            <joint name="panda_joint7" value="${pi/4}" />
+            <joint name="$(arg arm_id)_joint1" value="0" />
+            <joint name="$(arg arm_id)_joint2" value="0" />
+            <joint name="$(arg arm_id)_joint3" value="0" />
+            <joint name="$(arg arm_id)_joint4" value="-0.1" />
+            <joint name="$(arg arm_id)_joint5" value="0" />
+            <joint name="$(arg arm_id)_joint6" value="${pi}" />
+            <joint name="$(arg arm_id)_joint7" value="${pi/4}" />
         </group_state>
     </xacro:macro>
 
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
-    <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="panda_link0" />
+    <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="$(arg arm_id)_link0" />
 
     <xacro:macro name="collision" params="link">
         <!-- Enable (environmental) collisions of ${link}_sc -->
         <disable_default_collisions link="${link}_sc" />
         <!-- Disable collisions of link with any other arm link, as these are handled by the "sc" links -->
-        <disable_collisions link1="${link}" link2="panda_link0" reason="Never" />
-        <disable_collisions link1="${link}" link2="panda_link1" reason="Never" />
-        <disable_collisions link1="${link}" link2="panda_link2" reason="Never" />
-        <disable_collisions link1="${link}" link2="panda_link3" reason="Never" />
-        <disable_collisions link1="${link}" link2="panda_link4" reason="Never" />
-        <disable_collisions link1="${link}" link2="panda_link5" reason="Never" />
-        <disable_collisions link1="${link}" link2="panda_link6" reason="Never" />
-        <disable_collisions link1="${link}" link2="panda_link7" reason="Never" />
-        <disable_collisions link1="${link}" link2="panda_link8" reason="Never" />
+        <disable_collisions link1="${link}" link2="$(arg arm_id)_link0" reason="Never" />
+        <disable_collisions link1="${link}" link2="$(arg arm_id)_link1" reason="Never" />
+        <disable_collisions link1="${link}" link2="$(arg arm_id)_link2" reason="Never" />
+        <disable_collisions link1="${link}" link2="$(arg arm_id)_link3" reason="Never" />
+        <disable_collisions link1="${link}" link2="$(arg arm_id)_link4" reason="Never" />
+        <disable_collisions link1="${link}" link2="$(arg arm_id)_link5" reason="Never" />
+        <disable_collisions link1="${link}" link2="$(arg arm_id)_link6" reason="Never" />
+        <disable_collisions link1="${link}" link2="$(arg arm_id)_link7" reason="Never" />
+        <disable_collisions link1="${link}" link2="$(arg arm_id)_link8" reason="Never" />
     </xacro:macro>
-    <xacro:collision link="panda_link0"/>
-    <enable_collisions link1="panda_link0_sc" link2="panda_link5_sc" />
-    <enable_collisions link1="panda_link0_sc" link2="panda_link6_sc" />
-    <enable_collisions link1="panda_link0_sc" link2="panda_link7_sc" />
-    <enable_collisions link1="panda_link0_sc" link2="panda_link8_sc" />
-    <xacro:collision link="panda_link1" />
-    <enable_collisions link1="panda_link1_sc" link2="panda_link5_sc" />
-    <enable_collisions link1="panda_link1_sc" link2="panda_link6_sc" />
-    <enable_collisions link1="panda_link1_sc" link2="panda_link7_sc" />
-    <enable_collisions link1="panda_link1_sc" link2="panda_link8_sc" />
-    <xacro:collision link="panda_link2" />
-    <enable_collisions link1="panda_link2_sc" link2="panda_link5_sc" />
-    <enable_collisions link1="panda_link2_sc" link2="panda_link6_sc" />
-    <enable_collisions link1="panda_link2_sc" link2="panda_link7_sc" />
-    <enable_collisions link1="panda_link2_sc" link2="panda_link8_sc" />
-    <xacro:collision link="panda_link3" />
-    <enable_collisions link1="panda_link3_sc" link2="panda_link7_sc" />
-    <enable_collisions link1="panda_link3_sc" link2="panda_link8_sc" />
-    <xacro:collision link="panda_link4" />
-    <xacro:collision link="panda_link5" />
-    <xacro:collision link="panda_link6" />
-    <xacro:collision link="panda_link7" />
-    <xacro:collision link="panda_link8" />
+    <xacro:collision link="$(arg arm_id)_link0"/>
+    <enable_collisions link1="$(arg arm_id)_link0_sc" link2="$(arg arm_id)_link5_sc" />
+    <enable_collisions link1="$(arg arm_id)_link0_sc" link2="$(arg arm_id)_link6_sc" />
+    <enable_collisions link1="$(arg arm_id)_link0_sc" link2="$(arg arm_id)_link7_sc" />
+    <enable_collisions link1="$(arg arm_id)_link0_sc" link2="$(arg arm_id)_link8_sc" />
+    <xacro:collision link="$(arg arm_id)_link1" />
+    <enable_collisions link1="$(arg arm_id)_link1_sc" link2="$(arg arm_id)_link5_sc" />
+    <enable_collisions link1="$(arg arm_id)_link1_sc" link2="$(arg arm_id)_link6_sc" />
+    <enable_collisions link1="$(arg arm_id)_link1_sc" link2="$(arg arm_id)_link7_sc" />
+    <enable_collisions link1="$(arg arm_id)_link1_sc" link2="$(arg arm_id)_link8_sc" />
+    <xacro:collision link="$(arg arm_id)_link2" />
+    <enable_collisions link1="$(arg arm_id)_link2_sc" link2="$(arg arm_id)_link5_sc" />
+    <enable_collisions link1="$(arg arm_id)_link2_sc" link2="$(arg arm_id)_link6_sc" />
+    <enable_collisions link1="$(arg arm_id)_link2_sc" link2="$(arg arm_id)_link7_sc" />
+    <enable_collisions link1="$(arg arm_id)_link2_sc" link2="$(arg arm_id)_link8_sc" />
+    <xacro:collision link="$(arg arm_id)_link3" />
+    <enable_collisions link1="$(arg arm_id)_link3_sc" link2="$(arg arm_id)_link7_sc" />
+    <enable_collisions link1="$(arg arm_id)_link3_sc" link2="$(arg arm_id)_link8_sc" />
+    <xacro:collision link="$(arg arm_id)_link4" />
+    <xacro:collision link="$(arg arm_id)_link5" />
+    <xacro:collision link="$(arg arm_id)_link6" />
+    <xacro:collision link="$(arg arm_id)_link7" />
+    <xacro:collision link="$(arg arm_id)_link8" />
 </robot>

config/fake_controllers.yaml

@@ -1,31 +1,22 @@
 controller_list:
-  - name: fake_panda_arm_controller
+  - name: fake_$(arg arm_id)_arm_controller
     type: $(arg fake_execution_type)
     joints:
-      - panda_joint1
-      - panda_joint2
-      - panda_joint3
-      - panda_joint4
-      - panda_joint5
-      - panda_joint6
-      - panda_joint7
-  - name: fake_hand_controller
+      - $(arg arm_id)_joint1
+      - $(arg arm_id)_joint2
+      - $(arg arm_id)_joint3
+      - $(arg arm_id)_joint4
+      - $(arg arm_id)_joint5
+      - $(arg arm_id)_joint6
+      - $(arg arm_id)_joint7
+
+  - name: fake_$(arg arm_id)_hand_controller
     type: $(arg fake_execution_type)
     joints:
-      - panda_finger_joint1
-  - name: fake_panda_arm_hand_controller
-    type: $(arg fake_execution_type)
-    joints:
-      - panda_joint1
-      - panda_joint2
-      - panda_joint3
-      - panda_joint4
-      - panda_joint5
-      - panda_joint6
-      - panda_joint7
-      - panda_finger_joint1
+      - $(arg arm_id)_finger_joint1
+
 initial:  # Define initial robot poses per group
-  - group: panda_arm
+  - group: $(arg arm_id)_arm
     pose: ready
   - group: hand
     pose: open

config/hand.xacro

@@ -3,16 +3,16 @@
     <xacro:macro name="finger_collisions" params="finger">
         <!-- Disable all self-collisions between finger and arm links,
              as these are already covered by the coarser hand collision model -->
-        <disable_collisions link1="${finger}" link2="panda_link0" reason="Never" />
-        <disable_collisions link1="${finger}" link2="panda_link1" reason="Never" />
-        <disable_collisions link1="${finger}" link2="panda_link2" reason="Never" />
-        <disable_collisions link1="${finger}" link2="panda_link3" reason="Never" />
-        <disable_collisions link1="${finger}" link2="panda_link4" reason="Never" />
-        <disable_collisions link1="${finger}" link2="panda_link5" reason="Never" />
-        <disable_collisions link1="${finger}" link2="panda_link6" reason="Never" />
-        <disable_collisions link1="${finger}" link2="panda_link7" reason="Never" />
-        <disable_collisions link1="${finger}" link2="panda_link8" reason="Never" />
-        <disable_collisions link1="${finger}" link2="panda_hand" reason="Never" />
+        <disable_collisions link1="${finger}" link2="$(arg arm_id)_link0" reason="Never" />
+        <disable_collisions link1="${finger}" link2="$(arg arm_id)_link1" reason="Never" />
+        <disable_collisions link1="${finger}" link2="$(arg arm_id)_link2" reason="Never" />
+        <disable_collisions link1="${finger}" link2="$(arg arm_id)_link3" reason="Never" />
+        <disable_collisions link1="${finger}" link2="$(arg arm_id)_link4" reason="Never" />
+        <disable_collisions link1="${finger}" link2="$(arg arm_id)_link5" reason="Never" />
+        <disable_collisions link1="${finger}" link2="$(arg arm_id)_link6" reason="Never" />
+        <disable_collisions link1="${finger}" link2="$(arg arm_id)_link7" reason="Never" />
+        <disable_collisions link1="${finger}" link2="$(arg arm_id)_link8" reason="Never" />
+        <disable_collisions link1="${finger}" link2="$(arg arm_id)_hand" reason="Never" />
     </xacro:macro>
     <xacro:macro name="hand">
         <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
@@ -21,41 +21,41 @@
         <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
         <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
         <group name="hand">
-            <link name="panda_hand"/>
-            <link name="panda_leftfinger"/>
-            <link name="panda_rightfinger"/>
+            <link name="$(arg arm_id)_hand"/>
+            <link name="$(arg arm_id)_leftfinger"/>
+            <link name="$(arg arm_id)_rightfinger"/>
         </group>
         <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
         <group_state name="open" group="hand">
-            <joint name="panda_finger_joint1" value="0.035"/>
-            <joint name="panda_finger_joint2" value="0.035"/>
+            <joint name="$(arg arm_id)_finger_joint1" value="0.035"/>
+            <joint name="$(arg arm_id)_finger_joint2" value="0.035"/>
         </group_state>
         <group_state name="close" group="hand">
-            <joint name="panda_finger_joint1" value="0"/>
-            <joint name="panda_finger_joint2" value="0"/>
+            <joint name="$(arg arm_id)_finger_joint1" value="0"/>
+            <joint name="$(arg arm_id)_finger_joint2" value="0"/>
         </group_state>
         <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
         <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
         <!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
         <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
-        <disable_default_collisions link="panda_hand_sc" />
-        <enable_collisions link1="panda_hand_sc" link2="panda_link0_sc" />
-        <enable_collisions link1="panda_hand_sc" link2="panda_link1_sc" />
-        <enable_collisions link1="panda_hand_sc" link2="panda_link2_sc" />
-        <enable_collisions link1="panda_hand_sc" link2="panda_link3_sc" />
+        <disable_default_collisions link="$(arg arm_id)_hand_sc" />
+        <enable_collisions link1="$(arg arm_id)_hand_sc" link2="$(arg arm_id)_link0_sc" />
+        <enable_collisions link1="$(arg arm_id)_hand_sc" link2="$(arg arm_id)_link1_sc" />
+        <enable_collisions link1="$(arg arm_id)_hand_sc" link2="$(arg arm_id)_link2_sc" />
+        <enable_collisions link1="$(arg arm_id)_hand_sc" link2="$(arg arm_id)_link3_sc" />
         <!-- Disable collision of hand with all arm links. These are handled by the *_sc links -->
-        <disable_collisions link1="panda_hand" link2="panda_link0" reason="Never" />
-        <disable_collisions link1="panda_hand" link2="panda_link1" reason="Never" />
-        <disable_collisions link1="panda_hand" link2="panda_link2" reason="Never" />
-        <disable_collisions link1="panda_hand" link2="panda_link3" reason="Never" />
-        <disable_collisions link1="panda_hand" link2="panda_link4" reason="Never" />
-        <disable_collisions link1="panda_hand" link2="panda_link5" reason="Never" />
-        <disable_collisions link1="panda_hand" link2="panda_link6" reason="Never" />
-        <disable_collisions link1="panda_hand" link2="panda_link7" reason="Never" />
-        <disable_collisions link1="panda_hand" link2="panda_link8" reason="Never" />
+        <disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link0" reason="Never" />
+        <disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link1" reason="Never" />
+        <disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link2" reason="Never" />
+        <disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link3" reason="Never" />
+        <disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link4" reason="Never" />
+        <disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link5" reason="Never" />
+        <disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link6" reason="Never" />
+        <disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link7" reason="Never" />
+        <disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link8" reason="Never" />
 
-        <xacro:finger_collisions finger="panda_leftfinger" />
-        <xacro:finger_collisions finger="panda_rightfinger" />
-        <disable_collisions link1="panda_leftfinger" link2="panda_rightfinger" reason="Never" />
+        <xacro:finger_collisions finger="$(arg arm_id)_leftfinger" />
+        <xacro:finger_collisions finger="$(arg arm_id)_rightfinger" />
+        <disable_collisions link1="$(arg arm_id)_leftfinger" link2="$(arg arm_id)_rightfinger" reason="Never" />
     </xacro:macro>
 </robot>

config/joint_limits.yaml

@@ -14,47 +14,47 @@ default_acceleration_scaling_factor: 0.1
 # max_jerk = (max_acceleration - min_acceleration) / 0.001
 
 joint_limits:
-  panda_finger_joint1:
+  $(arg arm_id)_finger_joint1:
     has_velocity_limits: true
     max_velocity: 0.1
     has_acceleration_limits: false
     max_acceleration: 0
-  panda_finger_joint2:
+  $(arg arm_id)_finger_joint2:
     has_velocity_limits: true
     max_velocity: 0.1
     has_acceleration_limits: false
     max_acceleration: 0
-  panda_joint1:
+  $(arg arm_id)_joint1:
     has_velocity_limits: true
     max_velocity: 2.1750
     has_acceleration_limits: true
     max_acceleration: 3.75
-  panda_joint2:
+  $(arg arm_id)_joint2:
     has_velocity_limits: true
     max_velocity: 2.1750
     has_acceleration_limits: true
     max_acceleration: 1.875
-  panda_joint3:
+  $(arg arm_id)_joint3:
     has_velocity_limits: true
     max_velocity: 2.1750
     has_acceleration_limits: true
     max_acceleration: 2.5
-  panda_joint4:
+  $(arg arm_id)_joint4:
     has_velocity_limits: true
     max_velocity: 2.1750
     has_acceleration_limits: true
     max_acceleration: 3.125
-  panda_joint5:
+  $(arg arm_id)_joint5:
     has_velocity_limits: true
     max_velocity: 2.6100
     has_acceleration_limits: true
     max_acceleration: 3.75
-  panda_joint6:
+  $(arg arm_id)_joint6:
     has_velocity_limits: true
     max_velocity: 2.6100
     has_acceleration_limits: true
     max_acceleration: 5
-  panda_joint7:
+  $(arg arm_id)_joint7:
     has_velocity_limits: true
     max_velocity: 2.6100
     has_acceleration_limits: true

config/kinematics.yaml

@@ -1,4 +1,4 @@
-panda_arm: &arm_config
+$(arg arm_id)_arm: &arm_config
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
   kinematics_solver_timeout: 0.05

config/ompl_planning.yaml

@@ -127,7 +127,7 @@ planner_configs:
     sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
     dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
     max_failures: 5000  # maximum consecutive failure limit. default: 5000
-panda_arm: &arm_config
+$(arg arm_id)_arm: &arm_config
   planner_configs:
     - AnytimePathShortening
     - SBL
@@ -153,7 +153,7 @@ panda_arm: &arm_config
     - LazyPRMstar
     - SPARS
     - SPARStwo
-  projection_evaluator: joints(panda_joint1,panda_joint2)
+  projection_evaluator: joints($(arg arm_id)_joint1,$(arg arm_id)_joint2)
   longest_valid_segment_fraction: 0.005
 manipulator: *arm_config
 hand: