diff --git a/cob_moveit_config/robots/raw3-1/moveit/config/constraints_limit.yaml b/cob_moveit_config/robots/raw3-1/moveit/config/constraints_limit.yaml
new file mode 100644
index 000000000..e995a0f06
--- /dev/null
+++ b/cob_moveit_config/robots/raw3-1/moveit/config/constraints_limit.yaml
@@ -0,0 +1,90 @@
+
+##################################
+#joint constraint
+##################################
+
+joint_constraint_limits:
+# arm_elbow_joint:
+# center_position: 0
+# tolerance_above: 0
+# tolerance_below: 0
+# arm_shoulder_lift_joint:
+# center_position: 0
+# tolerance_above: 0
+# tolerance_below: 0
+ arm_shoulder_pan_joint:
+ center_position: -0.675
+ tolerance_above: 0.830
+ tolerance_below: 1.05
+ arm_wrist_1_joint:
+ center_position: -3.14
+ tolerance_above: 1.57
+ tolerance_below: 3.14
+ arm_wrist_2_joint:
+ center_position: -1.5
+ tolerance_above: 1.03
+ tolerance_below: 1.15
+# arm_wrist_3_joint:
+# center_position: 0
+# tolerance_above: 0
+# tolerance_below: 0
+
+##################################
+#position constraint --> check what required
+##################################
+
+#position_constraint_limits:
+# arm_elbow_joint:
+# center_position: 0
+# tolerance_above: 0
+# tolerance_below: 0
+# arm_shoulder_lift_joint:
+# center_position: 0
+# tolerance_above: 0
+# tolerance_below: 0
+# arm_shoulder_pan_joint:
+# center_position: 0
+# tolerance_above: 0
+# tolerance_below: 0
+# arm_wrist_1_joint:
+# center_position: 0
+# tolerance_above: 0
+# tolerance_below: 0
+# arm_wrist_2_joint:
+# center_position: -1.5
+# tolerance_above: 0.7652
+# tolerance_below: 1.15
+# arm_wrist_3_joint:
+# center_position: 0
+# tolerance_above: 0
+# tolerance_below: 0
+
+##################################
+#orientation constraint --> check what required
+##################################
+
+#joint_constraint_limits:
+# arm_elbow_joint:
+# center_position: -0.675
+# tolerance_above: 0.785
+# tolerance_below: 1.05
+# arm_shoulder_lift_joint:
+# center_position: 0
+# tolerance_above: 0
+# tolerance_below: 0
+# arm_shoulder_pan_joint:
+# center_position: 0
+# tolerance_above: 0
+# tolerance_below: 0
+# arm_wrist_1_joint:
+# center_position: -3.14
+# tolerance_above: 1.57
+# tolerance_below: 3.14
+# arm_wrist_2_joint:
+# center_position: -1.5
+# tolerance_above: 0.7652
+# tolerance_below: 1.15
+# arm_wrist_3_joint:
+# center_position: 0
+# tolerance_above: 0
+# tolerance_below: 0
diff --git a/cob_moveit_config/robots/raw3-1/moveit/config/raw3-1.srdf b/cob_moveit_config/robots/raw3-1/moveit/config/raw3-1.srdf
index a042b50e9..27685bdc1 100644
--- a/cob_moveit_config/robots/raw3-1/moveit/config/raw3-1.srdf
+++ b/cob_moveit_config/robots/raw3-1/moveit/config/raw3-1.srdf
@@ -10,15 +10,30 @@
-
-
-
-
+
+
+
+
+
+
+
+
-
+
-
+
+
+
+
+
+
+
+
+
+
@@ -36,29 +51,32 @@
-
-
-
-
-
+
+
+
+
+
-
-
-
-
+
+
+
-
+
+
+
+
+
@@ -75,36 +93,67 @@
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
+
+
+
-
+
+
+
-
+
+
+
+
-
+
+
+
+
-
+
+
+
+
@@ -117,12 +166,9 @@
-
-
-
-
-
+
+
@@ -135,12 +181,9 @@
-
-
-
-
-
+
+
@@ -152,12 +195,9 @@
-
-
-
-
-
+
+
@@ -168,12 +208,9 @@
-
-
-
-
-
+
+
@@ -183,12 +220,9 @@
-
-
-
-
-
+
+
@@ -197,12 +231,10 @@
-
-
-
+
-
-
+
+
@@ -211,96 +243,66 @@
-
-
-
-
-
+
+
-
-
-
-
-
+
+
-
-
-
-
-
-
+
+
-
-
-
+
+
-
-
+
+
-
-
-
+
-
-
+
+
-
-
-
-
-
+
+
-
-
-
-
-
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
-
-
-
+
+
+
diff --git a/cob_moveit_config/robots/raw3-1/moveit/config/stomp_config.yaml b/cob_moveit_config/robots/raw3-1/moveit/config/stomp_config.yaml
new file mode 100644
index 000000000..5d910c4c7
--- /dev/null
+++ b/cob_moveit_config/robots/raw3-1/moveit/config/stomp_config.yaml
@@ -0,0 +1,38 @@
+stomp/arm:
+ group_name: arm
+ optimization:
+ num_timesteps: 20
+ num_iterations: 50
+ num_iterations_after_valid: 0
+ num_rollouts: 10
+ max_rollouts: 100
+ 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.3, 1.0, 0.5, 0.4, 0.3, 0.3]
+ cost_functions:
+ - class: stomp_moveit/ObstacleDistanceGradient
+ max_distance: 0.2
+ cost_weight: 1.0
+ 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: [0, 255, 0]
+ marker_array_topic: stomp_trajectories
+ marker_namespace: noisy
+ update_filters:
+ - class: stomp_moveit/ControlCostProjectionMatrix
+ - class: stomp_moveit/TrajectoryVisualization
+ line_width: 0.02
+ rgb: [191, 0, 255]
+ error_rgb: [255, 0, 0]
+ publish_intermediate: True
+ marker_topic: stomp_trajectory
+ marker_namespace: optimized
+
diff --git a/cob_moveit_config/robots/raw3-1/moveit/config/stomp_planning.yaml b/cob_moveit_config/robots/raw3-1/moveit/config/stomp_planning.yaml
new file mode 100644
index 000000000..b8c96e28a
--- /dev/null
+++ b/cob_moveit_config/robots/raw3-1/moveit/config/stomp_planning.yaml
@@ -0,0 +1,49 @@
+# Optimization Task
+num_feature_basis_functions: 1
+trajectory_duration: 8.0
+num_time_steps: 30
+publish_trajectory_markers: False
+publish_best_trajectory_marker: True
+
+
+# STOMP
+max_iterations: 10
+max_iterations_after_collision_free: 1
+max_rollouts: 100
+min_rollouts: 30
+num_rollouts_per_iteration: 10
+start_state_max_bounds_error: 1.0
+start_state_max_dt: 2.0
+
+# STOMP (optional)
+use_noise_adaptation: true
+write_to_file: false
+use_openmp: false
+
+noise_coefficients:
+ - group_name: arm
+ stddev: [0.3, 1.0, 1.0, 0.8, 0.3, 0.3, 0.3]
+ min_stddev: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
+ decay: [0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8]
+ - group_name: robot
+ stddev: [1.2, 1.0, 1.0, 0.8, 0.3, 0.3]
+ min_stddev: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
+ decay: [0.8, 0.8, 0.8, 0.8, 0.8, 0.8]
+
+features:
+- class: stomp_moveit_interface/ObstacleAvoidanceFeature
+ collision_clearance: 0.02
+#- class: stomp_moveit_interface/CollisionFeature
+# report_validity: false # report state validity based on distance field
+# collision_clearance: 0.02
+# debug_collisions: false
+# collision_space:
+# size_x: 8.5 # The X dimension in meters of the volume to represent
+# size_y: 6.0 # The Y dimension in meters of the volume to represent
+# size_z: 4.0 # The Z dimension in meters of the volume to represent
+# origin_x: 0.0
+# origin_y: 0.0
+# origin_z: 1.5
+# resolution: 0.03
+# collision_tolerance: 0.01
+# max_propagation_distance: 0.1