From 9f0d06d14ba8470fa1ba56ead7df775a7414bc6c Mon Sep 17 00:00:00 2001
From: Xiaoyu27 <36901845+Xiaoyu27@users.noreply.github.com>
Date: Tue, 22 Mar 2022 17:33:14 +0100
Subject: [PATCH] Configurable arm_id (#106)
---
config/arm.xacro | 96 +++++++++++------------
config/fake_controllers.yaml | 35 +++------
config/hand.xacro | 68 ++++++++--------
config/joint_limits.yaml | 18 ++---
config/kinematics.yaml | 2 +-
config/ompl_planning.yaml | 4 +-
config/panda.srdf.xacro | 9 ++-
config/simple_moveit_controllers.yaml | 16 ++--
launch/demo.launch | 3 +-
launch/move_group.launch | 7 ++
launch/ompl_planning_pipeline.launch.xml | 3 +-
launch/planning_context.launch | 11 +--
launch/planning_pipeline.launch.xml | 3 +-
launch/ros_controllers.launch | 4 +-
launch/stomp_planning_pipeline.launch.xml | 4 +-
15 files changed, 143 insertions(+), 140 deletions(-)
diff --git a/config/arm.xacro b/config/arm.xacro
index f5be8b4..0556385 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 ef67f6e..6a6759c 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 1bea787..e9ec8e3 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 d531621..9bdaadd 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 9bbfb6d..ffeb7ae 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 b556110..69f40b0 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 4fa5062..99307dc 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 454ca42..19a874b 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 3a6db41..543cb19 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 7cb7e8e..7d511f3 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 00f8e4e..af9d56e 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 27d5a3a..d32922f 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 4b4d0d6..250e743 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 3d3b1f4..8fd5760 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 0f4b967..cbff8df 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" />
--
GitLab