diff --git a/config/arm.xacro b/config/arm.xacro
index f5be8b4f7801ffe1357bd152c47281d1317c9303..0556385fe5b20beb9668550d5e129ffb1d269c92 100644
--- a/config/arm.xacro
+++ b/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>
diff --git a/config/fake_controllers.yaml b/config/fake_controllers.yaml
index ef67f6eb2370c2fdb25ea39e028aab549f04b48b..6a6759c2b53c1bd62e75b3cff30758297b08f8a6 100644
--- a/config/fake_controllers.yaml
+++ b/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
\ No newline at end of file
diff --git a/config/hand.xacro b/config/hand.xacro
index 1bea7874b77f1f0bb23e68fdc54e119ca6067403..e9ec8e3a53f8a76cdf9cbd159a6a34a61797be3c 100644
--- a/config/hand.xacro
+++ b/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>
diff --git a/config/joint_limits.yaml b/config/joint_limits.yaml
index d531621597828461f32cea726ab7f211f266c430..9bdaadd3b806ccd8f9c46a096e648fd911958335 100644
--- a/config/joint_limits.yaml
+++ b/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
diff --git a/config/kinematics.yaml b/config/kinematics.yaml
index 9bbfb6d19b33882c4ef809689d303d31b40939e8..ffeb7ae785bb4d6305c06989908d4cc1c1e8eba6 100644
--- a/config/kinematics.yaml
+++ b/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
diff --git a/config/ompl_planning.yaml b/config/ompl_planning.yaml
index b556110df8bee711fb11c2bda566d017dd18181f..69f40b0b36bc0824ac2ee15f2624b58a362c4a5d 100644
--- a/config/ompl_planning.yaml
+++ b/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:
diff --git a/config/panda.srdf.xacro b/config/panda.srdf.xacro
index 4fa5062324144c84e22d28fafeed2fba41383fec..99307dc7f289a2f7815440fc52a03763ba5850f4 100644
--- a/config/panda.srdf.xacro
+++ b/config/panda.srdf.xacro
@@ -5,22 +5,23 @@
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
<xacro:include filename="$(find panda_moveit_config)/config/arm.xacro" />
+ <xacro:arg name="arm_id" default="panda" />
<!-- panda_arm group: eef frame aligned to robot's flanche -->
- <xacro:arm name="panda_arm" tip_link="panda_link8"/>
+ <xacro:arm name="$(arg arm_id)_arm" tip_link="$(arg arm_id)_link8"/>
<xacro:arg name="hand" default="false" />
<!--Add the hand if people request it-->
<xacro:if value="$(arg hand)">
<!-- manipulator group: eef frame aligned to hand -->
- <xacro:arm name="manipulator" tip_link="panda_hand_tcp" />
+ <xacro:arm name="$(arg arm_id)_manipulator" tip_link="$(arg arm_id)_hand_tcp" />
<xacro:include filename="$(find panda_moveit_config)/config/hand.xacro" />
<xacro:hand />
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
- <end_effector name="hand_tcp" parent_link="panda_hand_tcp" group="hand" parent_group="manipulator" />
+ <end_effector name="hand_tcp" parent_link="$(arg arm_id)_hand_tcp" group="hand" parent_group="$(arg arm_id)_manipulator" />
<!-- old end-effector -->
- <end_effector name="hand" parent_link="panda_link8" group="hand" parent_group="panda_arm" />
+ <end_effector name="hand" parent_link="$(arg arm_id)_link8" group="hand" parent_group="$(arg arm_id)_arm" />
</xacro:if>
</robot>
diff --git a/config/simple_moveit_controllers.yaml b/config/simple_moveit_controllers.yaml
index 454ca4230068ca37eb9e0de1fc8bee7f56f6070e..19a874be84d687824528a69378ab71a1385f151f 100644
--- a/config/simple_moveit_controllers.yaml
+++ b/config/simple_moveit_controllers.yaml
@@ -4,16 +4,16 @@ controller_list:
type: FollowJointTrajectory
default: True
joints:
- - panda_joint1
- - panda_joint2
- - panda_joint3
- - panda_joint4
- - panda_joint5
- - panda_joint6
- - panda_joint7
+ - $(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: franka_gripper
action_ns: gripper_action
type: GripperCommand
default: True
joints:
- - panda_finger_joint1
+ - $(arg arm_id)_finger_joint1
diff --git a/launch/demo.launch b/launch/demo.launch
index 3a6db415227257bcf602e62783e658247271f652..543cb190cef01eb75b87926eef33e75ee5f4847a 100644
--- a/launch/demo.launch
+++ b/launch/demo.launch
@@ -1,4 +1,5 @@
<launch>
+ <arg name="arm_id" default="panda" />
<!-- specify the planning pipeline -->
<arg name="pipeline" default="ompl" />
@@ -31,7 +32,7 @@
<arg name="rviz_tutorial" default="false" />
<!-- If needed, broadcast static tf for robot root -->
- <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world panda_link0" />
+ <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world $(arg arm_id)_link0" />
diff --git a/launch/move_group.launch b/launch/move_group.launch
index 7cb7e8efa3e406de31355b82dd332e43addfaf8d..7d511f3b4297e39511a5811f1f25c27d5aa2aa5f 100644
--- a/launch/move_group.launch
+++ b/launch/move_group.launch
@@ -24,6 +24,8 @@
<!--Other settings-->
<arg name="load_gripper" default="true" />
+ <arg name="transmission" default="effort" />
+ <arg name="arm_id" default="panda" />
<!-- load these non-default MoveGroup capabilities (space seperated) -->
<!--
@@ -46,6 +48,7 @@
<include file="$(dirname)/planning_context.launch">
<arg name="load_robot_description" value="$(arg load_robot_description)" />
<arg name="load_gripper" value="$(arg load_gripper)" />
+ <arg name="arm_id" value="$(arg arm_id)" />
</include>
<!-- Planning Pipelines -->
@@ -54,22 +57,26 @@
<!-- OMPL -->
<include file="$(dirname)/planning_pipeline.launch.xml">
<arg name="pipeline" value="ompl" />
+ <arg name="arm_id" value="$(arg arm_id)" />
</include>
<!-- CHOMP -->
<include file="$(dirname)/planning_pipeline.launch.xml">
<arg name="pipeline" value="chomp" />
+ <arg name="arm_id" value="$(arg arm_id)" />
</include>
<!-- Pilz Industrial Motion -->
<include file="$(dirname)/planning_pipeline.launch.xml">
<arg name="pipeline" value="pilz_industrial_motion_planner" />
+ <arg name="arm_id" value="$(arg arm_id)" />
</include>
<!-- Support custom planning pipeline -->
<include if="$(eval arg('pipeline') not in ['ompl', 'chomp', 'pilz_industrial_motion_planner'])"
file="$(dirname)/planning_pipeline.launch.xml">
<arg name="pipeline" value="$(arg pipeline)" />
+ <arg name="arm_id" value="$(arg arm_id)" />
</include>
</group>
diff --git a/launch/ompl_planning_pipeline.launch.xml b/launch/ompl_planning_pipeline.launch.xml
index 00f8e4e9e5445bd085e15b452c2a696ded8be7f7..af9d56e1964f4b9721b1e07d804aae393b8c89e7 100644
--- a/launch/ompl_planning_pipeline.launch.xml
+++ b/launch/ompl_planning_pipeline.launch.xml
@@ -12,6 +12,7 @@
<arg name="start_state_max_bounds_error" default="0.1" />
<arg name="jiggle_fraction" default="0.05" />
+ <arg name="arm_id" default="panda" />
<param name="planning_plugin" value="ompl_interface/OMPLPlanner" />
<param name="request_adapters" value="$(arg planning_adapters)" />
@@ -21,6 +22,6 @@
<!-- Add MoveGroup capabilities specific to this pipeline -->
<!-- <param name="capabilities" value="" /> -->
- <rosparam command="load" file="$(find panda_moveit_config)/config/ompl_planning.yaml"/>
+ <rosparam command="load" file="$(find panda_moveit_config)/config/ompl_planning.yaml" subst_value="true"/>
</launch>
diff --git a/launch/planning_context.launch b/launch/planning_context.launch
index 27d5a3ae72a50d8dc2969aa45f00e6a577857463..d32922f6c70862fd5f837028cc2cc2cefb031ed1 100644
--- a/launch/planning_context.launch
+++ b/launch/planning_context.launch
@@ -2,25 +2,26 @@
<!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
<arg name="load_gripper" default="true" />
<arg name="load_robot_description" default="false"/>
+ <arg name="arm_id" default="panda" />
<!-- The name of the parameter under which the URDF is loaded -->
<arg name="robot_description" default="robot_description"/>
<!-- Load universal robot description format (URDF) -->
- <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="xacro hand:=$(arg load_gripper) '$(find franka_description)/robots/panda_arm.urdf.xacro'"/>
+ <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="xacro hand:=$(arg load_gripper) '$(find franka_description)/robots/panda_arm.urdf.xacro' arm_id:=$(arg arm_id)"/>
<!-- The semantic description that corresponds to the URDF -->
- <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find panda_moveit_config)/config/panda.srdf.xacro' hand:=$(arg load_gripper)"/>
+ <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find panda_moveit_config)/config/panda.srdf.xacro' hand:=$(arg load_gripper) arm_id:=$(arg arm_id)"/>
<!-- Load updated joint limits (override information from URDF) -->
<group ns="$(arg robot_description)_planning">
- <rosparam command="load" file="$(find panda_moveit_config)/config/joint_limits.yaml"/>
- <rosparam command="load" file="$(find panda_moveit_config)/config/cartesian_limits.yaml"/>
+ <rosparam command="load" file="$(find panda_moveit_config)/config/joint_limits.yaml" subst_value="true" />
+ <rosparam command="load" file="$(find panda_moveit_config)/config/cartesian_limits.yaml" subst_value="true"/>
</group>
<!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
<group ns="$(arg robot_description)_kinematics">
- <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
+ <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml" subst_value="true"/>
</group>
diff --git a/launch/planning_pipeline.launch.xml b/launch/planning_pipeline.launch.xml
index 4b4d0d663ae2476ae11b2fcfbda689331ebe6fbf..250e7432c09689f81976582238058667197bb90d 100644
--- a/launch/planning_pipeline.launch.xml
+++ b/launch/planning_pipeline.launch.xml
@@ -4,7 +4,8 @@
It is assumed that all planning pipelines are named XXX_planning_pipeline.launch -->
<arg name="pipeline" default="ompl" />
+ <arg name="arm_id" default="panda" />
- <include ns="$(arg pipeline)" file="$(dirname)/$(arg pipeline)_planning_pipeline.launch.xml" />
+ <include ns="$(arg pipeline)" file="$(dirname)/$(arg pipeline)_planning_pipeline.launch.xml" pass_all_args="true"/>
</launch>
diff --git a/launch/ros_controllers.launch b/launch/ros_controllers.launch
index 3d3b1f4d1095a4b05f77584249629ef5a4d9f5ca..8fd5760e31519a16e994a42dc6c102d4a2755cd3 100644
--- a/launch/ros_controllers.launch
+++ b/launch/ros_controllers.launch
@@ -2,9 +2,9 @@
<launch>
<!-- Load joint controller configurations from YAML file to parameter server -->
- <rosparam file="$(find panda_moveit_config)/config/ros_controllers.yaml" command="load"/>
+ <rosparam file="$(find panda_moveit_config)/config/ros_controllers.yaml" command="load" subst_value="true"/>
<!-- Load and start the controllers -->
- <node ns="/" name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
+ <node ns="/$(arg arm_id)" name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
args="$(arg transmission)_joint_trajectory_controller" />
</launch>
diff --git a/launch/stomp_planning_pipeline.launch.xml b/launch/stomp_planning_pipeline.launch.xml
index 0f4b9675839e4e685216ec604a79c3bb63545985..cbff8df91f8595c47a9f1b9eb92f9ad4c43eff99 100644
--- a/launch/stomp_planning_pipeline.launch.xml
+++ b/launch/stomp_planning_pipeline.launch.xml
@@ -22,9 +22,9 @@
<!-- <param name="capabilities" value="" /> -->
<!-- Load parameters for both groups, panda_arm and manipulator -->
- <group ns="panda_arm">
+ <group ns="$(arg arm_id)_arm">
<rosparam command="load" file="$(find panda_moveit_config)/config/stomp_planning.yaml" />
- <param name="group_name" value="panda_arm" />
+ <param name="group_name" value="$(arg arm_id)_arm" />
</group>
<group ns="manipulator">
<rosparam command="load" file="$(find panda_moveit_config)/config/stomp_planning.yaml" />