diff --git a/cob_bringup/robots/raw3-1.xml b/cob_bringup/robots/raw3-1.xml
index 0dff1556a..3fc8a598b 100644
--- a/cob_bringup/robots/raw3-1.xml
+++ b/cob_bringup/robots/raw3-1.xml
@@ -67,6 +67,8 @@
 			<arg name="sim" value="$(arg sim)"/>
 			<arg if="$(arg sim)" name="cartesian_control" value="false"/>
 		</include>
+		<!-- Schmalz CobotPump driver -->
+		<include file="$(find schmalz_ecbpi)/launch/schmalz_ecbpi_server.launch" />
 
 		<!-- start common nodes used in hardware and simulation -->
 		<include file="$(find cob_bringup)/drivers/scan_unifier.launch">
diff --git a/cob_hardware_config/robots/raw3-1/urdf/calibration_default.urdf.xacro b/cob_hardware_config/robots/raw3-1/urdf/calibration_default.urdf.xacro
index 8ddffadc7..d618d04a9 100644
--- a/cob_hardware_config/robots/raw3-1/urdf/calibration_default.urdf.xacro
+++ b/cob_hardware_config/robots/raw3-1/urdf/calibration_default.urdf.xacro
@@ -98,4 +98,12 @@
 	<xacro:property name="def_cam3d_pitch" value="0.0"/>
 	<xacro:property name="def_cam3d_yaw" value="0.0"/>
 
+        <!-- arm camera | handeye calibration | relative to cam_reference_link -->
+        <xacro:property name="def_arm_camera_x" value="0.083"/>
+        <xacro:property name="def_arm_camera_y" value="0.0"/>
+        <xacro:property name="def_arm_camera_z" value="0.0"/>
+        <xacro:property name="def_arm_camera_roll" value="0.0"/>
+        <xacro:property name="def_arm_camera_pitch" value="-1.57"/>
+        <xacro:property name="def_arm_camera_yaw" value="-3.1415"/>
+
 </robot>
diff --git a/cob_hardware_config/robots/raw3-1/urdf/properties.urdf.xacro b/cob_hardware_config/robots/raw3-1/urdf/properties.urdf.xacro
index 96173c827..d84a6167f 100644
--- a/cob_hardware_config/robots/raw3-1/urdf/properties.urdf.xacro
+++ b/cob_hardware_config/robots/raw3-1/urdf/properties.urdf.xacro
@@ -100,4 +100,13 @@
 	<xacro:property name="cam3d_roll" value="${def_cam3d_roll+offset_cam3d_roll}"/>
 	<xacro:property name="cam3d_pitch" value="${def_cam3d_pitch+offset_cam3d_pitch}"/>
 	<xacro:property name="cam3d_yaw" value="${def_cam3d_yaw+offset_cam3d_yaw}"/>
+
+        <!-- arm camera | handeye calibration | relative to cam_reference_link -->
+        <xacro:property name="arm_camera_x" value="${def_arm_camera_x}"/>
+        <xacro:property name="arm_camera_y" value="${def_arm_camera_y}"/>
+        <xacro:property name="arm_camera_z" value="${def_arm_camera_z}"/>
+        <xacro:property name="arm_camera_roll" value="${def_arm_camera_roll}"/>
+        <xacro:property name="arm_camera_pitch" value="${def_arm_camera_pitch}"/>
+        <xacro:property name="arm_camera_yaw" value="${def_arm_camera_yaw}"/>
+
 </robot>
diff --git a/cob_hardware_config/robots/raw3-1/urdf/raw3-1.urdf.xacro b/cob_hardware_config/robots/raw3-1/urdf/raw3-1.urdf.xacro
index 2362e469a..3a6fc05f6 100644
--- a/cob_hardware_config/robots/raw3-1/urdf/raw3-1.urdf.xacro
+++ b/cob_hardware_config/robots/raw3-1/urdf/raw3-1.urdf.xacro
@@ -20,8 +20,11 @@
   <!-- ur_arm -->
   <xacro:include filename="$(find raw_description)/urdf/ur_arm/ur10_arm.urdf.xacro" />
 
-  <!-- vacuum gripper -->
-  <xacro:include filename="$(find raw_description)/urdf/vacuum_gripper/gripper.urdf.xacro" />
+  <!-- schmaltz vacuum gripper -->
+  <xacro:include filename="$(find raw_description)/urdf/schmaltz_gripper/gripper.urdf.xacro" />
+
+  <!-- arm camera -->
+  <xacro:include filename="$(find raw_description)/urdf/kinect_camera/kinect_camera.urdf.xacro" />
 
   <!-- sensors -->
   <xacro:include filename="$(find cob_description)/urdf/sensors/sick_s300_laser.urdf.xacro"/>
@@ -54,4 +57,9 @@
     <origin xyz="${gripper_x} ${gripper_y} ${gripper_z}" rpy="${gripper_roll} ${gripper_pitch} ${gripper_yaw}" />
   </xacro:vacuum_gripper>
 
+  <xacro:kinect_camera name="arm_camera" ros_topic="arm_kinect_camera" parent="gripper_coupling_link" update_rate="30" data_skip="9">
+    <origin xyz="${arm_camera_x} ${arm_camera_y} ${arm_camera_z}" rpy="${arm_camera_roll} ${arm_camera_pitch} ${arm_camera_yaw}" />
+  </xacro:kinect_camera>
+  
+
 </robot>
diff --git a/cob_moveit_config/robots/raw3-1/moveit/config/joint_limits.yaml b/cob_moveit_config/robots/raw3-1/moveit/config/joint_limits.yaml
index eb39c9728..a196e3c98 100644
--- a/cob_moveit_config/robots/raw3-1/moveit/config/joint_limits.yaml
+++ b/cob_moveit_config/robots/raw3-1/moveit/config/joint_limits.yaml
@@ -4,41 +4,31 @@
 joint_limits:
   arm_elbow_joint:
     has_velocity_limits: true
-    max_velocity: 1.0
-    has_acceleration_limits: true
-    max_acceleration: 1.0
+    max_velocity: 2
+    has_acceleration_limits: false
+    max_acceleration: 0
   arm_shoulder_lift_joint:
     has_velocity_limits: true
-    max_velocity: 1.0
-    has_acceleration_limits: true
-    max_acceleration: 1.0
+    max_velocity: 2
+    has_acceleration_limits: false
+    max_acceleration: 0
   arm_shoulder_pan_joint:
     has_velocity_limits: true
-    max_velocity: 1.0
-    has_acceleration_limits: true
-    max_acceleration: 1.0
+    max_velocity: 2
+    has_acceleration_limits: false
+    max_acceleration: 0
   arm_wrist_1_joint:
     has_velocity_limits: true
-    max_velocity: 1.0
-    has_acceleration_limits: true
-    max_acceleration: 1.0
+    max_velocity: 2
+    has_acceleration_limits: false
+    max_acceleration: 0
   arm_wrist_2_joint:
     has_velocity_limits: true
-    max_velocity: 1.0
-    has_acceleration_limits: true
-    max_acceleration: 1.0
+    max_velocity: 2
+    has_acceleration_limits: false
+    max_acceleration: 0
   arm_wrist_3_joint:
     has_velocity_limits: true
-    max_velocity: 1.0
-    has_acceleration_limits: true
-    max_acceleration: 1.0
-  torso_pan_joint:
-    has_velocity_limits: true
-    max_velocity: 1.0
-    has_acceleration_limits: true
-    max_acceleration: 1.0
-  torso_tilt_joint:
-    has_velocity_limits: true
-    max_velocity: 1.0
-    has_acceleration_limits: true
-    max_acceleration: 1.0
+    max_velocity: 2
+    has_acceleration_limits: false
+    max_acceleration: 0
\ No newline at end of file
diff --git a/cob_moveit_config/robots/raw3-1/moveit/config/ompl_planning.yaml b/cob_moveit_config/robots/raw3-1/moveit/config/ompl_planning.yaml
index 126020c0d..49b1199c4 100644
--- a/cob_moveit_config/robots/raw3-1/moveit/config/ompl_planning.yaml
+++ b/cob_moveit_config/robots/raw3-1/moveit/config/ompl_planning.yaml
@@ -26,7 +26,7 @@ planner_configs:
     min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
   RRTkConfigDefault:
     type: geometric::RRT
-    range: 0.1  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
     goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
   RRTConnectkConfigDefault:
     type: geometric::RRTConnect
@@ -64,4 +64,4 @@ arm:
     - RRTstarkConfigDefault
     - TRRTkConfigDefault
     - PRMkConfigDefault
-    - PRMstarkConfigDefault
+    - PRMstarkConfigDefault
\ No newline at end of file
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..b92d5e763 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,23 @@
     <!--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" />
+        <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" />
-    <!--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" />
+    <!--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 +44,60 @@
     <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_camera_collision_link" link2="arm_camera_link" reason="Default" />
+    <disable_collisions link1="arm_camera_collision_link" link2="arm_ee_link" reason="Never" />
+    <disable_collisions link1="arm_camera_collision_link" link2="arm_wrist_1_link" reason="Never" />
+    <disable_collisions link1="arm_camera_collision_link" link2="arm_wrist_2_link" reason="Never" />
+    <disable_collisions link1="arm_camera_collision_link" link2="arm_wrist_3_link" reason="Default" />
+    <disable_collisions link1="arm_camera_collision_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="arm_camera_collision_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_camera_collision_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_camera_collision_link" link2="gripper" reason="Never" />
+    <disable_collisions link1="arm_camera_collision_link" link2="gripper_body" reason="Default" />
+    <disable_collisions link1="arm_camera_collision_link" link2="gripper_coupling_link" reason="Adjacent" />
+    <disable_collisions link1="arm_camera_collision_link" link2="torso_0_link" reason="Never" />
+    <disable_collisions link1="arm_camera_collision_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="arm_camera_collision_link" link2="torso_knob_link" reason="Never" />
+    <disable_collisions link1="arm_camera_link" link2="arm_ee_link" reason="Never" />
+    <disable_collisions link1="arm_camera_link" link2="arm_wrist_1_link" reason="Never" />
+    <disable_collisions link1="arm_camera_link" link2="arm_wrist_2_link" reason="Never" />
+    <disable_collisions link1="arm_camera_link" link2="arm_wrist_3_link" reason="Default" />
+    <disable_collisions link1="arm_camera_link" link2="bl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_camera_link" link2="br_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_camera_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="arm_camera_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_camera_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="arm_camera_link" link2="gripper" reason="Never" />
+    <disable_collisions link1="arm_camera_link" link2="gripper_body" reason="Default" />
+    <disable_collisions link1="arm_camera_link" link2="gripper_coupling_link" reason="Adjacent" />
+    <disable_collisions link1="arm_camera_link" link2="torso_0_link" reason="Never" />
+    <disable_collisions link1="arm_camera_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="arm_camera_link" link2="torso_knob_link" 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_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="gripper_body" reason="Default" />
+    <disable_collisions link1="arm_ee_link" link2="gripper_coupling_link" reason="Adjacent" />
+    <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 +114,55 @@
     <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_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_coupling_link" 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="Default" />
+    <disable_collisions link1="arm_wrist_2_link" link2="gripper_coupling_link" 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_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="gripper_coupling_link" 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 +175,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 +190,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 +204,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 +217,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 +229,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 +240,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 +252,73 @@
     <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="gripper_coupling_link" 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="gripper_coupling_link" 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="gripper_coupling_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="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="gripper" link2="gripper_body" reason="Never" />
+    <disable_collisions link1="gripper" link2="gripper_coupling_link" 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="gripper_coupling_link" reason="Adjacent" />
+    <disable_collisions link1="gripper_body" link2="torso_0_link" reason="Never" />
+    <disable_collisions link1="gripper_body" link2="torso_knob_link" reason="Never" />
+    <disable_collisions link1="gripper_coupling_link" link2="torso_0_link" reason="Never" />
+    <disable_collisions link1="gripper_coupling_link" link2="torso_ball_link" reason="Never" />
+    <disable_collisions link1="gripper_coupling_link" 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="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>