Skip to content

Commit

Permalink
Adds stomp planner
Browse files Browse the repository at this point in the history
This commit adds all the scripts needed to run the STOMP planner
example in the
[stomp_planner_tutorial](https://ros-planning.github.io/moveit_tutorials/doc/stomp_planner/stomp_planner_tutorial.html).
  • Loading branch information
rickstaa committed Aug 25, 2021
1 parent 47073b3 commit 4d57c3d
Show file tree
Hide file tree
Showing 2 changed files with 65 additions and 0 deletions.
40 changes: 40 additions & 0 deletions config/stomp_planning.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,40 @@

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
25 changes: 25 additions & 0 deletions launch/stomp_planning_pipeline.launch.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
<launch>
<!-- Stomp Plugin for MoveIt! -->
<arg name="planning_plugin" value="stomp_moveit/StompPlannerManager" />

<arg name="start_state_max_bounds_error" value="0.1" />
<arg name="jiggle_fraction" value="0.05" />
<!-- The request adapters (plugins) used when planning.
ORDER MATTERS -->
<arg name="planning_adapters" default="default_planner_request_adapters/AddTimeParameterization
default_planner_request_adapters/FixWorkspaceBounds
default_planner_request_adapters/FixStartStateBounds
default_planner_request_adapters/FixStartStateCollision
default_planner_request_adapters/FixStartStatePathConstraints" />


<param name="planning_plugin" value="$(arg planning_plugin)" />
<param name="request_adapters" value="$(arg planning_adapters)" />
<param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
<param name="jiggle_fraction" value="$(arg jiggle_fraction)" />

<!-- Add MoveGroup capabilities specific to this pipeline -->
<!-- <param name="capabilities" value="" /> -->

<rosparam command="load" file="$(find panda_moveit_config)/config/stomp_planning.yaml"/>
</launch>

0 comments on commit 4d57c3d

Please sign in to comment.