Use STOMP config for both arm groups

larics · Mar 15, 2022 · e7a0f47 · e7a0f47
1 parent e60dc0b
commit e7a0f47
Show file tree

Hide file tree

Showing 2 changed files with 47 additions and 41 deletions.
diff --git a/config/stomp_planning.yaml b/config/stomp_planning.yaml
@@ -1,40 +1,37 @@
-
-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
+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
diff --git a/launch/stomp_planning_pipeline.launch.xml b/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>