From e5b0b13279b378fb881d519e13b7f9c1843f6259 Mon Sep 17 00:00:00 2001
From: Richard Bormann <Richard.Bormann@ipa.fraunhofer.de>
Date: Thu, 3 May 2018 15:57:16 +0200
Subject: [PATCH] added updated configuration files of raw3-1

---
 .../moveit/config/constraints_limit.yaml      |  90 +++++++
 .../robots/raw3-1/moveit/config/raw3-1.srdf   | 232 +++++++++---------
 .../raw3-1/moveit/config/stomp_config.yaml    |  38 +++
 .../raw3-1/moveit/config/stomp_planning.yaml  |  49 ++++
 4 files changed, 294 insertions(+), 115 deletions(-)
 create mode 100644 cob_moveit_config/robots/raw3-1/moveit/config/constraints_limit.yaml
 create mode 100644 cob_moveit_config/robots/raw3-1/moveit/config/stomp_config.yaml
 create mode 100644 cob_moveit_config/robots/raw3-1/moveit/config/stomp_planning.yaml

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 @@
     <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
     <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
     <group name="arm">
-        <chain base_link="arm_base_link" tip_link="arm_ee_link" />
-    </group>
-    <group name="endeffector">
-        <link name="arm_ee_link"/>
+        <joint name="arm_shoulder_pan_joint" />
+        <joint name="arm_shoulder_lift_joint" />
+        <joint name="arm_elbow_joint" />
+        <joint name="arm_wrist_1_joint" />
+        <joint name="arm_wrist_2_joint" />
+        <joint name="arm_wrist_3_joint" />
+        <chain base_link="base_link" tip_link="gripper" />
     </group>
+    <!--group name="endeffector">
+        <link name="arm_ee_link" />
+     </group-->
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
-    <end_effector name="moveit_ee" parent_link="arm_ee_link" group="endeffector" />
+    <!--end_effector name="moveit_ee" parent_link="arm_ee_link" group="endeffector" /-->
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
-    <virtual_joint name="base" type="planar" parent_frame="odom_combined" child_link="base_footprint" />
+    <!--virtual_joint name="base" type="planar" parent_frame="map" child_link="base_link" /-->
+    <!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
+    <passive_joint name="bl_caster_rotation_joint" />
+    <passive_joint name="bl_caster_r_wheel_joint" />
+    <passive_joint name="br_caster_rotation_joint" />
+    <passive_joint name="br_caster_r_wheel_joint" />
+    <passive_joint name="fl_caster_rotation_joint" />
+    <passive_joint name="fl_caster_r_wheel_joint" />
+    <passive_joint name="fr_caster_rotation_joint" />
+    <passive_joint name="fr_caster_r_wheel_joint" />
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <disable_collisions link1="arm_base_link" link2="arm_shoulder_link" reason="Adjacent" />
     <disable_collisions link1="arm_base_link" link2="arm_upper_arm_link" reason="Never" />
@@ -36,29 +51,32 @@
     <disable_collisions link1="arm_base_link" link2="fl_caster_rotation_link" reason="Never" />
     <disable_collisions link1="arm_base_link" link2="fr_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="arm_base_link" link2="fr_caster_rotation_link" reason="Never" />
-    <disable_collisions link1="arm_base_link" link2="head_cam3d_link" reason="Never" />
-    <disable_collisions link1="arm_base_link" link2="head_color_camera_l_link" reason="Never" />
-    <disable_collisions link1="arm_base_link" link2="head_color_camera_r_link" reason="Never" />
     <disable_collisions link1="arm_base_link" link2="torso_0_link" reason="Never" />
-    <disable_collisions link1="arm_base_link" link2="torso_1_link" reason="Never" />
-    <disable_collisions link1="arm_base_link" link2="torso_2_link" reason="Never" />
+    <disable_collisions link1="arm_base_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="arm_base_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="arm_base_link" link2="tower_base_link" reason="Never" />
+    <disable_collisions link1="arm_ee_link" link2="arm_tool0" reason="Never" />
     <disable_collisions link1="arm_ee_link" link2="arm_wrist_1_link" reason="Never" />
     <disable_collisions link1="arm_ee_link" link2="arm_wrist_2_link" reason="Never" />
     <disable_collisions link1="arm_ee_link" link2="arm_wrist_3_link" reason="Adjacent" />
     <disable_collisions link1="arm_ee_link" link2="bl_caster_rotation_link" reason="Never" />
     <disable_collisions link1="arm_ee_link" link2="br_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_ee_link" link2="fl_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="arm_ee_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_ee_link" link2="fr_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="arm_ee_link" link2="fr_caster_rotation_link" reason="Never" />
     <disable_collisions link1="arm_ee_link" link2="gripper" reason="Adjacent" />
     <disable_collisions link1="arm_ee_link" link2="gripper_body" reason="Adjacent" />
-    <disable_collisions link1="arm_ee_link" link2="head_cam3d_link" reason="Never" />
-    <disable_collisions link1="arm_ee_link" link2="head_color_camera_l_link" reason="Never" />
-    <disable_collisions link1="arm_ee_link" link2="head_color_camera_r_link" reason="Never" />
-    <disable_collisions link1="arm_ee_link" link2="torso_2_link" reason="Never" />
+    <disable_collisions link1="arm_ee_link" link2="torso_0_link" reason="Never" />
+    <disable_collisions link1="arm_ee_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="arm_ee_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="arm_forearm_link" link2="arm_upper_arm_link" reason="Adjacent" />
     <disable_collisions link1="arm_forearm_link" link2="arm_wrist_1_link" reason="Adjacent" />
-    <disable_collisions link1="arm_forearm_link" link2="bl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_forearm_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="arm_forearm_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_forearm_link" link2="torso_0_link" reason="Never" />
+    <disable_collisions link1="arm_forearm_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="arm_forearm_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="arm_shoulder_link" link2="arm_upper_arm_link" reason="Adjacent" />
     <disable_collisions link1="arm_shoulder_link" link2="arm_wrist_1_link" reason="Never" />
     <disable_collisions link1="arm_shoulder_link" link2="arm_wrist_2_link" reason="Never" />
@@ -75,36 +93,67 @@
     <disable_collisions link1="arm_shoulder_link" link2="fl_caster_rotation_link" reason="Never" />
     <disable_collisions link1="arm_shoulder_link" link2="fr_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="arm_shoulder_link" link2="fr_caster_rotation_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_link" link2="head_cam3d_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_link" link2="head_color_camera_l_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_link" link2="head_color_camera_r_link" reason="Never" />
     <disable_collisions link1="arm_shoulder_link" link2="torso_0_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_link" link2="torso_1_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_link" link2="torso_2_link" reason="Never" />
+    <disable_collisions link1="arm_shoulder_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="arm_shoulder_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="arm_shoulder_link" link2="tower_base_link" reason="Never" />
+    <disable_collisions link1="arm_tool0" link2="arm_wrist_1_link" reason="Never" />
+    <disable_collisions link1="arm_tool0" link2="arm_wrist_2_link" reason="Never" />
+    <disable_collisions link1="arm_tool0" link2="arm_wrist_3_link" reason="Adjacent" />
+    <disable_collisions link1="arm_tool0" link2="bl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_tool0" link2="br_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_tool0" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="arm_tool0" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_tool0" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_tool0" link2="gripper" reason="Default" />
+    <disable_collisions link1="arm_tool0" link2="gripper_body" reason="Default" />
+    <disable_collisions link1="arm_tool0" link2="torso_0_link" reason="Never" />
+    <disable_collisions link1="arm_tool0" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="arm_tool0" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="arm_upper_arm_link" link2="base_laser_rear_link" reason="Never" />
     <disable_collisions link1="arm_upper_arm_link" link2="bl_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="arm_upper_arm_link" link2="bl_caster_rotation_link" reason="Never" />
     <disable_collisions link1="arm_upper_arm_link" link2="br_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="arm_upper_arm_link" link2="br_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_upper_arm_link" link2="fl_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="arm_upper_arm_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_upper_arm_link" link2="fr_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="arm_upper_arm_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_upper_arm_link" link2="torso_0_link" reason="Never" />
+    <disable_collisions link1="arm_upper_arm_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="arm_upper_arm_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="arm_wrist_1_link" link2="arm_wrist_2_link" reason="Adjacent" />
     <disable_collisions link1="arm_wrist_1_link" link2="arm_wrist_3_link" reason="Never" />
     <disable_collisions link1="arm_wrist_1_link" link2="br_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_wrist_1_link" link2="fl_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="arm_wrist_1_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_wrist_1_link" link2="fr_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="arm_wrist_1_link" link2="fr_caster_rotation_link" reason="Never" />
     <disable_collisions link1="arm_wrist_1_link" link2="gripper" reason="Never" />
-    <disable_collisions link1="gripper_body" link2="arm_wrist_1_link" reason="Never" />
+    <disable_collisions link1="arm_wrist_1_link" link2="gripper_body" reason="Never" />
+    <disable_collisions link1="arm_wrist_1_link" link2="torso_0_link" reason="Never" />
+    <disable_collisions link1="arm_wrist_1_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="arm_wrist_1_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="arm_wrist_2_link" link2="arm_wrist_3_link" reason="Adjacent" />
-    <disable_collisions link1="arm_wrist_2_link" link2="bl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_wrist_2_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="arm_wrist_2_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_wrist_2_link" link2="fr_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="arm_wrist_2_link" link2="fr_caster_rotation_link" reason="Never" />
     <disable_collisions link1="arm_wrist_2_link" link2="gripper" reason="Never" />
-    <disable_collisions link1="gripper_body" link2="arm_wrist_2_link" reason="Never" />
+    <disable_collisions link1="arm_wrist_2_link" link2="gripper_body" reason="Never" />
+    <disable_collisions link1="arm_wrist_2_link" link2="torso_0_link" reason="Never" />
+    <disable_collisions link1="arm_wrist_2_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="arm_wrist_2_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="arm_wrist_3_link" link2="bl_caster_rotation_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_3_link" link2="br_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_wrist_3_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="arm_wrist_3_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_wrist_3_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="arm_wrist_3_link" link2="fr_caster_rotation_link" reason="Never" />
     <disable_collisions link1="arm_wrist_3_link" link2="gripper" reason="Never" />
-    <disable_collisions link1="gripper_body" link2="arm_wrist_3_link" reason="Never" />
+    <disable_collisions link1="arm_wrist_3_link" link2="gripper_body" reason="Default" />
+    <disable_collisions link1="arm_wrist_3_link" link2="torso_0_link" reason="Never" />
+    <disable_collisions link1="arm_wrist_3_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="arm_wrist_3_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="base_battery_front_battery" link2="base_battery_rear_battery" reason="Never" />
     <disable_collisions link1="base_battery_front_battery" link2="base_laser_front_link" reason="Never" />
     <disable_collisions link1="base_battery_front_battery" link2="base_laser_rear_link" reason="Never" />
@@ -117,12 +166,9 @@
     <disable_collisions link1="base_battery_front_battery" link2="fl_caster_rotation_link" reason="Never" />
     <disable_collisions link1="base_battery_front_battery" link2="fr_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="base_battery_front_battery" link2="fr_caster_rotation_link" reason="Never" />
-    <disable_collisions link1="base_battery_front_battery" link2="head_cam3d_link" reason="Never" />
-    <disable_collisions link1="base_battery_front_battery" link2="head_color_camera_l_link" reason="Never" />
-    <disable_collisions link1="base_battery_front_battery" link2="head_color_camera_r_link" reason="Never" />
     <disable_collisions link1="base_battery_front_battery" link2="torso_0_link" reason="Never" />
-    <disable_collisions link1="base_battery_front_battery" link2="torso_1_link" reason="Never" />
-    <disable_collisions link1="base_battery_front_battery" link2="torso_2_link" reason="Never" />
+    <disable_collisions link1="base_battery_front_battery" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="base_battery_front_battery" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="base_battery_front_battery" link2="tower_base_link" reason="Never" />
     <disable_collisions link1="base_battery_rear_battery" link2="base_laser_front_link" reason="Never" />
     <disable_collisions link1="base_battery_rear_battery" link2="base_laser_rear_link" reason="Never" />
@@ -135,12 +181,9 @@
     <disable_collisions link1="base_battery_rear_battery" link2="fl_caster_rotation_link" reason="Never" />
     <disable_collisions link1="base_battery_rear_battery" link2="fr_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="base_battery_rear_battery" link2="fr_caster_rotation_link" reason="Never" />
-    <disable_collisions link1="base_battery_rear_battery" link2="head_cam3d_link" reason="Never" />
-    <disable_collisions link1="base_battery_rear_battery" link2="head_color_camera_l_link" reason="Never" />
-    <disable_collisions link1="base_battery_rear_battery" link2="head_color_camera_r_link" reason="Never" />
     <disable_collisions link1="base_battery_rear_battery" link2="torso_0_link" reason="Never" />
-    <disable_collisions link1="base_battery_rear_battery" link2="torso_1_link" reason="Never" />
-    <disable_collisions link1="base_battery_rear_battery" link2="torso_2_link" reason="Never" />
+    <disable_collisions link1="base_battery_rear_battery" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="base_battery_rear_battery" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="base_battery_rear_battery" link2="tower_base_link" reason="Never" />
     <disable_collisions link1="base_laser_front_link" link2="base_laser_rear_link" reason="Never" />
     <disable_collisions link1="base_laser_front_link" link2="base_link" reason="Adjacent" />
@@ -152,12 +195,9 @@
     <disable_collisions link1="base_laser_front_link" link2="fl_caster_rotation_link" reason="Never" />
     <disable_collisions link1="base_laser_front_link" link2="fr_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="base_laser_front_link" link2="fr_caster_rotation_link" reason="Never" />
-    <disable_collisions link1="base_laser_front_link" link2="head_cam3d_link" reason="Never" />
-    <disable_collisions link1="base_laser_front_link" link2="head_color_camera_l_link" reason="Never" />
-    <disable_collisions link1="base_laser_front_link" link2="head_color_camera_r_link" reason="Never" />
     <disable_collisions link1="base_laser_front_link" link2="torso_0_link" reason="Never" />
-    <disable_collisions link1="base_laser_front_link" link2="torso_1_link" reason="Never" />
-    <disable_collisions link1="base_laser_front_link" link2="torso_2_link" reason="Never" />
+    <disable_collisions link1="base_laser_front_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="base_laser_front_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="base_laser_front_link" link2="tower_base_link" reason="Never" />
     <disable_collisions link1="base_laser_rear_link" link2="base_link" reason="Adjacent" />
     <disable_collisions link1="base_laser_rear_link" link2="bl_caster_r_wheel_link" reason="Never" />
@@ -168,12 +208,9 @@
     <disable_collisions link1="base_laser_rear_link" link2="fl_caster_rotation_link" reason="Never" />
     <disable_collisions link1="base_laser_rear_link" link2="fr_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="base_laser_rear_link" link2="fr_caster_rotation_link" reason="Never" />
-    <disable_collisions link1="base_laser_rear_link" link2="head_cam3d_link" reason="Never" />
-    <disable_collisions link1="base_laser_rear_link" link2="head_color_camera_l_link" reason="Never" />
-    <disable_collisions link1="base_laser_rear_link" link2="head_color_camera_r_link" reason="Never" />
     <disable_collisions link1="base_laser_rear_link" link2="torso_0_link" reason="Never" />
-    <disable_collisions link1="base_laser_rear_link" link2="torso_1_link" reason="Never" />
-    <disable_collisions link1="base_laser_rear_link" link2="torso_2_link" reason="Never" />
+    <disable_collisions link1="base_laser_rear_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="base_laser_rear_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="base_laser_rear_link" link2="tower_base_link" reason="Never" />
     <disable_collisions link1="base_link" link2="bl_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="base_link" link2="bl_caster_rotation_link" reason="Adjacent" />
@@ -183,12 +220,9 @@
     <disable_collisions link1="base_link" link2="fl_caster_rotation_link" reason="Adjacent" />
     <disable_collisions link1="base_link" link2="fr_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="base_link" link2="fr_caster_rotation_link" reason="Adjacent" />
-    <disable_collisions link1="base_link" link2="head_cam3d_link" reason="Never" />
-    <disable_collisions link1="base_link" link2="head_color_camera_l_link" reason="Never" />
-    <disable_collisions link1="base_link" link2="head_color_camera_r_link" reason="Never" />
     <disable_collisions link1="base_link" link2="torso_0_link" reason="Adjacent" />
-    <disable_collisions link1="base_link" link2="torso_1_link" reason="Never" />
-    <disable_collisions link1="base_link" link2="torso_2_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="base_link" link2="tower_base_link" reason="Adjacent" />
     <disable_collisions link1="bl_caster_r_wheel_link" link2="bl_caster_rotation_link" reason="Adjacent" />
     <disable_collisions link1="bl_caster_r_wheel_link" link2="br_caster_r_wheel_link" reason="Never" />
@@ -197,12 +231,10 @@
     <disable_collisions link1="bl_caster_r_wheel_link" link2="fl_caster_rotation_link" reason="Never" />
     <disable_collisions link1="bl_caster_r_wheel_link" link2="fr_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="bl_caster_r_wheel_link" link2="fr_caster_rotation_link" reason="Never" />
-    <disable_collisions link1="bl_caster_r_wheel_link" link2="head_cam3d_link" reason="Never" />
-    <disable_collisions link1="bl_caster_r_wheel_link" link2="head_color_camera_l_link" reason="Never" />
-    <disable_collisions link1="bl_caster_r_wheel_link" link2="head_color_camera_r_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="gripper" reason="Never" />
     <disable_collisions link1="bl_caster_r_wheel_link" link2="torso_0_link" reason="Never" />
-    <disable_collisions link1="bl_caster_r_wheel_link" link2="torso_1_link" reason="Never" />
-    <disable_collisions link1="bl_caster_r_wheel_link" link2="torso_2_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="bl_caster_r_wheel_link" link2="tower_base_link" reason="Never" />
     <disable_collisions link1="bl_caster_rotation_link" link2="br_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="bl_caster_rotation_link" link2="br_caster_rotation_link" reason="Never" />
@@ -211,96 +243,66 @@
     <disable_collisions link1="bl_caster_rotation_link" link2="fr_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="bl_caster_rotation_link" link2="fr_caster_rotation_link" reason="Never" />
     <disable_collisions link1="bl_caster_rotation_link" link2="gripper" reason="Never" />
-    <disable_collisions link1="bl_caster_rotation_link" link2="head_cam3d_link" reason="Never" />
-    <disable_collisions link1="bl_caster_rotation_link" link2="head_color_camera_l_link" reason="Never" />
-    <disable_collisions link1="bl_caster_rotation_link" link2="head_color_camera_r_link" reason="Never" />
     <disable_collisions link1="bl_caster_rotation_link" link2="torso_0_link" reason="Never" />
-    <disable_collisions link1="bl_caster_rotation_link" link2="torso_1_link" reason="Never" />
-    <disable_collisions link1="bl_caster_rotation_link" link2="torso_2_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="bl_caster_rotation_link" link2="tower_base_link" reason="Never" />
     <disable_collisions link1="br_caster_r_wheel_link" link2="br_caster_rotation_link" reason="Adjacent" />
     <disable_collisions link1="br_caster_r_wheel_link" link2="fl_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="br_caster_r_wheel_link" link2="fl_caster_rotation_link" reason="Never" />
     <disable_collisions link1="br_caster_r_wheel_link" link2="fr_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="br_caster_r_wheel_link" link2="fr_caster_rotation_link" reason="Never" />
-    <disable_collisions link1="br_caster_r_wheel_link" link2="head_cam3d_link" reason="Never" />
-    <disable_collisions link1="br_caster_r_wheel_link" link2="head_color_camera_l_link" reason="Never" />
-    <disable_collisions link1="br_caster_r_wheel_link" link2="head_color_camera_r_link" reason="Never" />
     <disable_collisions link1="br_caster_r_wheel_link" link2="torso_0_link" reason="Never" />
-    <disable_collisions link1="br_caster_r_wheel_link" link2="torso_1_link" reason="Never" />
-    <disable_collisions link1="br_caster_r_wheel_link" link2="torso_2_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="br_caster_r_wheel_link" link2="tower_base_link" reason="Never" />
     <disable_collisions link1="br_caster_rotation_link" link2="fl_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="br_caster_rotation_link" link2="fl_caster_rotation_link" reason="Never" />
     <disable_collisions link1="br_caster_rotation_link" link2="fr_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="br_caster_rotation_link" link2="fr_caster_rotation_link" reason="Never" />
     <disable_collisions link1="br_caster_rotation_link" link2="gripper" reason="Never" />
-    <disable_collisions link1="br_caster_rotation_link" link2="gripper_body" reason="Never" />
-    <disable_collisions link1="br_caster_rotation_link" link2="head_cam3d_link" reason="Never" />
-    <disable_collisions link1="br_caster_rotation_link" link2="head_color_camera_l_link" reason="Never" />
-    <disable_collisions link1="br_caster_rotation_link" link2="head_color_camera_r_link" reason="Never" />
     <disable_collisions link1="br_caster_rotation_link" link2="torso_0_link" reason="Never" />
-    <disable_collisions link1="br_caster_rotation_link" link2="torso_1_link" reason="Never" />
-    <disable_collisions link1="br_caster_rotation_link" link2="torso_2_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="br_caster_rotation_link" link2="tower_base_link" reason="Never" />
     <disable_collisions link1="fl_caster_r_wheel_link" link2="fl_caster_rotation_link" reason="Adjacent" />
     <disable_collisions link1="fl_caster_r_wheel_link" link2="fr_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="fl_caster_r_wheel_link" link2="fr_caster_rotation_link" reason="Never" />
-    <disable_collisions link1="fl_caster_r_wheel_link" link2="head_cam3d_link" reason="Never" />
-    <disable_collisions link1="fl_caster_r_wheel_link" link2="head_color_camera_l_link" reason="Never" />
-    <disable_collisions link1="fl_caster_r_wheel_link" link2="head_color_camera_r_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="gripper" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="gripper_body" reason="Never" />
     <disable_collisions link1="fl_caster_r_wheel_link" link2="torso_0_link" reason="Never" />
-    <disable_collisions link1="fl_caster_r_wheel_link" link2="torso_1_link" reason="Never" />
-    <disable_collisions link1="fl_caster_r_wheel_link" link2="torso_2_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="fl_caster_r_wheel_link" link2="tower_base_link" reason="Never" />
     <disable_collisions link1="fl_caster_rotation_link" link2="fr_caster_r_wheel_link" reason="Never" />
     <disable_collisions link1="fl_caster_rotation_link" link2="fr_caster_rotation_link" reason="Never" />
     <disable_collisions link1="fl_caster_rotation_link" link2="gripper" reason="Never" />
-    <disable_collisions link1="fl_caster_rotation_link" link2="head_cam3d_link" reason="Never" />
-    <disable_collisions link1="fl_caster_rotation_link" link2="head_color_camera_l_link" reason="Never" />
-    <disable_collisions link1="fl_caster_rotation_link" link2="head_color_camera_r_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="gripper_body" reason="Never" />
     <disable_collisions link1="fl_caster_rotation_link" link2="torso_0_link" reason="Never" />
-    <disable_collisions link1="fl_caster_rotation_link" link2="torso_1_link" reason="Never" />
-    <disable_collisions link1="fl_caster_rotation_link" link2="torso_2_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="fl_caster_rotation_link" link2="tower_base_link" reason="Never" />
     <disable_collisions link1="fr_caster_r_wheel_link" link2="fr_caster_rotation_link" reason="Adjacent" />
-    <disable_collisions link1="fr_caster_r_wheel_link" link2="head_cam3d_link" reason="Never" />
-    <disable_collisions link1="fr_caster_r_wheel_link" link2="head_color_camera_l_link" reason="Never" />
-    <disable_collisions link1="fr_caster_r_wheel_link" link2="head_color_camera_r_link" reason="Never" />
     <disable_collisions link1="fr_caster_r_wheel_link" link2="torso_0_link" reason="Never" />
-    <disable_collisions link1="fr_caster_r_wheel_link" link2="torso_1_link" reason="Never" />
-    <disable_collisions link1="fr_caster_r_wheel_link" link2="torso_2_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="fr_caster_r_wheel_link" link2="tower_base_link" reason="Never" />
     <disable_collisions link1="fr_caster_rotation_link" link2="gripper" reason="Never" />
-    <disable_collisions link1="fr_caster_rotation_link" link2="head_cam3d_link" reason="Never" />
-    <disable_collisions link1="fr_caster_rotation_link" link2="head_color_camera_l_link" reason="Never" />
-    <disable_collisions link1="fr_caster_rotation_link" link2="head_color_camera_r_link" reason="Never" />
     <disable_collisions link1="fr_caster_rotation_link" link2="torso_0_link" reason="Never" />
-    <disable_collisions link1="fr_caster_rotation_link" link2="torso_1_link" reason="Never" />
-    <disable_collisions link1="fr_caster_rotation_link" link2="torso_2_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="fr_caster_rotation_link" link2="tower_base_link" reason="Never" />
-    <disable_collisions link1="gripper_body" link2="gripper" reason="Adjacent" />
-    <disable_collisions link1="gripper" link2="head_cam3d_link" reason="Never" />
-    <disable_collisions link1="gripper" link2="head_color_camera_l_link" reason="Never" />
-    <disable_collisions link1="gripper" link2="head_color_camera_r_link" reason="Never" />
-    <disable_collisions link1="gripper" link2="torso_2_link" reason="Never" />
-    <disable_collisions link1="head_cam3d_link" link2="head_color_camera_l_link" reason="Adjacent" />
-    <disable_collisions link1="head_cam3d_link" link2="head_color_camera_r_link" reason="Adjacent" />
-    <disable_collisions link1="head_cam3d_link" link2="torso_1_link" reason="Default" />
-    <disable_collisions link1="head_cam3d_link" link2="torso_2_link" reason="Adjacent" />
-    <disable_collisions link1="head_cam3d_link" link2="tower_base_link" reason="Never" />
-    <disable_collisions link1="head_color_camera_l_link" link2="head_color_camera_r_link" reason="Adjacent" />
-    <disable_collisions link1="head_color_camera_l_link" link2="torso_0_link" reason="Never" />
-    <disable_collisions link1="head_color_camera_l_link" link2="torso_1_link" reason="Default" />
-    <disable_collisions link1="head_color_camera_l_link" link2="torso_2_link" reason="Adjacent" />
-    <disable_collisions link1="head_color_camera_l_link" link2="tower_base_link" reason="Never" />
-    <disable_collisions link1="head_color_camera_r_link" link2="torso_1_link" reason="Default" />
-    <disable_collisions link1="head_color_camera_r_link" link2="torso_2_link" reason="Adjacent" />
-    <disable_collisions link1="head_color_camera_r_link" link2="tower_base_link" reason="Never" />
-    <disable_collisions link1="torso_0_link" link2="torso_1_link" reason="Adjacent" />
-    <disable_collisions link1="torso_0_link" link2="torso_2_link" reason="Never" />
+    <disable_collisions link1="gripper" link2="gripper_body" reason="Adjacent" />
+    <disable_collisions link1="gripper" link2="torso_0_link" reason="Never" />
+    <disable_collisions link1="gripper" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="gripper" link2="torso_knob_link" reason="Never" />
+    <disable_collisions link1="gripper_body" link2="torso_0_link" reason="Never" />
+    <disable_collisions link1="gripper_body" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="gripper_body" link2="torso_knob_link" reason="Never" />
+    <disable_collisions link1="torso_0_link" link2="torso_ball_link" reason="Adjacent" />
+    <disable_collisions link1="torso_0_link" link2="torso_knob_link" reason="Never" />
     <disable_collisions link1="torso_0_link" link2="tower_base_link" reason="Default" />
-    <disable_collisions link1="torso_1_link" link2="torso_2_link" reason="Adjacent" />
-    <disable_collisions link1="torso_1_link" link2="tower_base_link" reason="Never" />
-    <disable_collisions link1="torso_2_link" link2="tower_base_link" reason="Never" />
+    <disable_collisions link1="torso_ball_link" link2="torso_knob_link" reason="Adjacent" />
+    <disable_collisions link1="torso_ball_link" link2="tower_base_link" reason="Never" />
+    <disable_collisions link1="torso_knob_link" link2="tower_base_link" reason="Never" />
 </robot>
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