diff --git a/config/stomp_planning.yaml b/config/stomp_planning.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d6d34365efff05644420676a2f9c79f9cdaf1677
--- /dev/null
+++ b/config/stomp_planning.yaml
@@ -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
\ No newline at end of file
diff --git a/launch/demo_stomp.launch b/launch/demo_stomp.launch
new file mode 100644
index 0000000000000000000000000000000000000000..0ae05517da11fc70561bed8e6c1a8a29ad0a3dee
--- /dev/null
+++ b/launch/demo_stomp.launch
@@ -0,0 +1,5 @@
+<launch>
+  <include file="$(dirname)/demo.launch">
+    <arg name="pipeline" value="stomp"/>
+  </include>
+</launch>
diff --git a/launch/stomp_planning_pipeline.launch.xml b/launch/stomp_planning_pipeline.launch.xml
new file mode 100644
index 0000000000000000000000000000000000000000..06b42dc0efeefc4dc180b99dfa825d5de0c6afac
--- /dev/null
+++ b/launch/stomp_planning_pipeline.launch.xml
@@ -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>