diff --git a/config/stomp_planning.yaml b/config/stomp_planning.yaml index d6d34365efff05644420676a2f9c79f9cdaf1677..7084f2fd8c251f07c77ddc2115b315d94577c7ec 100644 --- 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 \ 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 diff --git a/launch/stomp_planning_pipeline.launch.xml b/launch/stomp_planning_pipeline.launch.xml index 06b42dc0efeefc4dc180b99dfa825d5de0c6afac..0f4b9675839e4e685216ec604a79c3bb63545985 100644 --- 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>