update according to upstream

config/arm.xacro

@@ -35,26 +35,17 @@
 
     <xacro:macro name="collision" params="link">
         <!-- Enable (environmental) collisions of ${link}_sc -->
-        <collision_default link="${link}_sc" allow="ALWAYS" />
+        <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="Default" />
-        <disable_collisions link1="${link}" link2="panda_link1" reason="Default" />
-        <disable_collisions link1="${link}" link2="panda_link2" reason="Default" />
-        <disable_collisions link1="${link}" link2="panda_link3" reason="Default" />
-        <disable_collisions link1="${link}" link2="panda_link4" reason="Default" />
-        <disable_collisions link1="${link}" link2="panda_link5" reason="Default" />
-        <disable_collisions link1="${link}" link2="panda_link6" reason="Default" />
-        <disable_collisions link1="${link}" link2="panda_link7" reason="Default" />
-        <disable_collisions link1="${link}" link2="panda_link8" reason="Default" />
-        <disable_collisions link1="${link}_sc" link2="panda_link0" reason="Default" />
-        <disable_collisions link1="${link}_sc" link2="panda_link1" reason="Default" />
-        <disable_collisions link1="${link}_sc" link2="panda_link2" reason="Default" />
-        <disable_collisions link1="${link}_sc" link2="panda_link3" reason="Default" />
-        <disable_collisions link1="${link}_sc" link2="panda_link4" reason="Default" />
-        <disable_collisions link1="${link}_sc" link2="panda_link5" reason="Default" />
-        <disable_collisions link1="${link}_sc" link2="panda_link6" reason="Default" />
-        <disable_collisions link1="${link}_sc" link2="panda_link7" reason="Default" />
-        <disable_collisions link1="${link}_sc" link2="panda_link8" reason="Default" />
+        <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" />
     </xacro:macro>
     <xacro:collision link="panda_link0"/>
     <enable_collisions link1="panda_link0_sc" link2="panda_link5_sc" />

config/hand.xacro

@@ -3,17 +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="Default" />
-        <disable_collisions link1="${finger}" link2="panda_link1" reason="Default" />
-        <disable_collisions link1="${finger}" link2="panda_link2" reason="Default" />
-        <disable_collisions link1="${finger}" link2="panda_link3" reason="Default" />
-        <disable_collisions link1="${finger}" link2="panda_link4" reason="Default" />
-        <disable_collisions link1="${finger}" link2="panda_link5" reason="Default" />
-        <disable_collisions link1="${finger}" link2="panda_link6" reason="Default" />
-        <disable_collisions link1="${finger}" link2="panda_link7" reason="Default" />
-        <disable_collisions link1="${finger}" link2="panda_link8" reason="Default" />
-        <disable_collisions link1="${finger}" link2="panda_hand" reason="Default" />
-        <disable_collisions link1="${finger}" link2="panda_hand_sc" reason="Default" />
+        <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" />
     </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-->
@@ -39,24 +38,24 @@
         <!--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. -->
-        <collision_default link="panda_hand_sc" allow="ALWAYS" />
-        <enable_collisions link1="panda_hand_sc" link2="panda_link0" />
-        <enable_collisions link1="panda_hand_sc" link2="panda_link1" />
-        <enable_collisions link1="panda_hand_sc" link2="panda_link2" />
-        <enable_collisions link1="panda_hand_sc" link2="panda_link3" />
+        <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 collision of hand with all arm links. These are handled by the *_sc links -->
-        <disable_collisions link1="panda_hand" link2="panda_link0" />
-        <disable_collisions link1="panda_hand" link2="panda_link1" />
-        <disable_collisions link1="panda_hand" link2="panda_link2" />
-        <disable_collisions link1="panda_hand" link2="panda_link3" />
-        <disable_collisions link1="panda_hand" link2="panda_link4" />
-        <disable_collisions link1="panda_hand" link2="panda_link5" />
-        <disable_collisions link1="panda_hand" link2="panda_link6" />
-        <disable_collisions link1="panda_hand" link2="panda_link7" />
-        <disable_collisions link1="panda_hand" link2="panda_link8" />
+        <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" />
 
         <xacro:finger_collisions finger="panda_leftfinger" />
         <xacro:finger_collisions finger="panda_rightfinger" />
-        <disable_collisions link1="panda_leftfinger" link2="panda_rightfinger" reason="Default" />
+        <disable_collisions link1="panda_leftfinger" link2="panda_rightfinger" reason="Never" />
     </xacro:macro>
 </robot>

config/kinematics.yaml

-panda_arm:
+panda_arm: &arm_config
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
-  kinematics_solver_timeout: 0.05
\ No newline at end of file
+  kinematics_solver_timeout: 0.05
+
+manipulator: *arm_config

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:
+panda_arm: &arm_config
   planner_configs:
     - AnytimePathShortening
     - SBL
@@ -155,6 +155,7 @@ panda_arm:
     - SPARStwo
   projection_evaluator: joints(panda_joint1,panda_joint2)
   longest_valid_segment_fraction: 0.005
+manipulator: *arm_config
 hand:
   planner_configs:
     - AnytimePathShortening
@@ -181,31 +182,3 @@ hand:
     - LazyPRMstar
     - SPARS
     - SPARStwo
-panda_arm_hand:
-  planner_configs:
-    - AnytimePathShortening
-    - SBL
-    - EST
-    - LBKPIECE
-    - BKPIECE
-    - KPIECE
-    - RRT
-    - RRTConnect
-    - RRTstar
-    - TRRT
-    - PRM
-    - PRMstar
-    - FMT
-    - BFMT
-    - PDST
-    - STRIDE
-    - BiTRRT
-    - LBTRRT
-    - BiEST
-    - ProjEST
-    - LazyPRM
-    - LazyPRMstar
-    - SPARS
-    - SPARStwo
-  projection_evaluator: joints(panda_joint1,panda_joint2)
-  longest_valid_segment_fraction: 0.005

config/panda.srdf.xacro

@@ -5,14 +5,15 @@
 -->
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
     <xacro:include filename="$(find panda_moveit_config)/config/arm.xacro" />
-    <xacro:arm name="manipulator" tip_link="panda_hand_tcp" />
-    <!-- old group name with old end-effector link -->
+    <!-- panda_arm group: eef frame aligned to robot's flanche -->
     <xacro:arm name="panda_arm" tip_link="panda_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:include filename="$(find panda_moveit_config)/config/hand.xacro" />
         <xacro:hand />
 

config/stomp_planning.yaml

-
-stomp/panda_arm:
-  group_name: panda_arm
-  optimization:
-    num_timesteps: 60
-    num_iterations: 40
-    num_iterations_after_valid: 0
-    num_rollouts: 30
-    max_rollouts: 30
-    initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]
-    control_cost_weight: 0.0
-  task:
-    noise_generator:
-      - class: stomp_moveit/NormalDistributionSampling
-        stddev: [0.05, 0.8, 1.0, 0.8, 0.4, 0.4, 0.4]
-    cost_functions:
-      - class: stomp_moveit/CollisionCheck
-        collision_penalty: 1.0
-        cost_weight: 1.0
-        kernel_window_percentage: 0.2
-        longest_valid_joint_move: 0.05
-    noisy_filters:
-      - class: stomp_moveit/JointLimits
-        lock_start: True
-        lock_goal: True
-      - class: stomp_moveit/MultiTrajectoryVisualization
-        line_width: 0.02
-        rgb: [255, 255, 0]
-        marker_array_topic: stomp_trajectories
-        marker_namespace: noisy
-    update_filters:
-      - class: stomp_moveit/PolynomialSmoother
-        poly_order: 6
-      - class: stomp_moveit/TrajectoryVisualization
-        line_width: 0.05
-        rgb: [0, 191, 255]
-        error_rgb: [255, 0, 0]
-        publish_intermediate: True
-        marker_topic: stomp_trajectory
-        marker_namespace: optimized
\ No newline at end of file
+optimization:
+  num_timesteps: 60
+  num_iterations: 40
+  num_iterations_after_valid: 0
+  num_rollouts: 30
+  max_rollouts: 30
+  initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]
+  control_cost_weight: 0.0
+task:
+  noise_generator:
+    - class: stomp_moveit/NormalDistributionSampling
+      stddev: [0.05, 0.8, 1.0, 0.8, 0.4, 0.4, 0.4]
+  cost_functions:
+    - class: stomp_moveit/CollisionCheck
+      collision_penalty: 1.0
+      cost_weight: 1.0
+      kernel_window_percentage: 0.2
+      longest_valid_joint_move: 0.05
+  noisy_filters:
+    - class: stomp_moveit/JointLimits
+      lock_start: True
+      lock_goal: True
+    - class: stomp_moveit/MultiTrajectoryVisualization
+      line_width: 0.02
+      rgb: [255, 255, 0]
+      marker_array_topic: stomp_trajectories
+      marker_namespace: noisy
+  update_filters:
+    - class: stomp_moveit/PolynomialSmoother
+      poly_order: 6
+    - class: stomp_moveit/TrajectoryVisualization
+      line_width: 0.05
+      rgb: [0, 191, 255]
+      error_rgb: [255, 0, 0]
+      publish_intermediate: True
+      marker_topic: stomp_trajectory
+      marker_namespace: optimized

launch/ros_controllers.launch

@@ -6,5 +6,5 @@
 
   <!-- Load and start the controllers -->
   <node ns="/" name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
-        args="$(arg transmission)_joint_trajectory_controller franka_gripper" />
+        args="$(arg transmission)_joint_trajectory_controller" />
 </launch>

launch/stomp_planning_pipeline.launch.xml

@@ -21,5 +21,14 @@
   <!-- Add MoveGroup capabilities specific to this pipeline -->
   <!-- <param name="capabilities" value="" /> -->
 
-  <rosparam command="load" file="$(find panda_moveit_config)/config/stomp_planning.yaml"/>
+  <!-- Load parameters for both groups, panda_arm and manipulator -->
+  <group ns="panda_arm">
+    <rosparam command="load" file="$(find panda_moveit_config)/config/stomp_planning.yaml" />
+    <param name="group_name" value="panda_arm" />
+  </group>
+  <group ns="manipulator">
+    <rosparam command="load" file="$(find panda_moveit_config)/config/stomp_planning.yaml" />
+    <param name="group_name" value="manipulator" />
+  </group>
+
 </launch>