diff --git a/rbs_mill_assist/urdf/rbs_arm.xacro b/rbs_mill_assist/urdf/rbs_arm.xacro
index 554d9f0..4ababe3 100644
--- a/rbs_mill_assist/urdf/rbs_arm.xacro
+++ b/rbs_mill_assist/urdf/rbs_arm.xacro
@@ -8,7 +8,7 @@
 
   <xacro:arg name="x" default="-0.10" />
   <xacro:arg name="y" default="0.0" />
-  <xacro:arg name="z" default="0.03" />
+  <xacro:arg name="z" default="0.0" />
 
   <xacro:arg name="roll" default="0.0" />
   <xacro:arg name="pitch" default="0.0" />
diff --git a/rbs_mill_assist/urdf/rbs_arm_macro.xacro b/rbs_mill_assist/urdf/rbs_arm_macro.xacro
index a4446d2..e6c17c1 100644
--- a/rbs_mill_assist/urdf/rbs_arm_macro.xacro
+++ b/rbs_mill_assist/urdf/rbs_arm_macro.xacro
@@ -160,7 +160,7 @@
       <child
         link="Link2" />
       <axis
-        xyz="-1 0 0" />
+        xyz="1 0 0" />
       <limit
         lower="-2.2689"
         upper="1.7453"
@@ -276,7 +276,7 @@
       <child
         link="Link4" />
       <axis
-        xyz="0 0 -1" />
+        xyz="0 0 1" />
       <limit
         lower="-6.2832"
         upper="6.2832"
@@ -505,17 +505,45 @@
       <axis
         xyz="0 0 0" />
     </joint>
+    <link name="grasp_point"/>
+    <joint
+      name="grasp_point_joint"
+      type="fixed">
+      <origin
+        xyz="0.0 -0.095 0.05"
+        rpy="0 0 0" />
+      <parent
+        link="Link6" />
+      <child
+        link="grasp_point" />
+      <axis
+        xyz="0 0 0" />
+    </joint>
+
+    <xacro:macro name="gazebo_reference" params="joint_name">
+      <gazebo reference='${joint_name}'>
+        <preserveFixedJoint>true</preserveFixedJoint>
+      </gazebo>
+    </xacro:macro>
+
+    <xacro:gazebo_reference joint_name="Right_point"/>
+    <xacro:gazebo_reference joint_name="Left_point"/>
 
 
     <xacro:joint_hardware joint_name="ax1" hardware="${hardware}" initial_joint_position="0.0" />
-    <xacro:joint_hardware joint_name="ax2" hardware="${hardware}" initial_joint_position="0.0" />
-    <xacro:joint_hardware joint_name="ax3" hardware="${hardware}" initial_joint_position="0.0" />
+    <xacro:joint_hardware joint_name="ax2" hardware="${hardware}" initial_joint_position="0.85" />
+    <xacro:joint_hardware joint_name="ax3" hardware="${hardware}" initial_joint_position="1.0" />
     <xacro:joint_hardware joint_name="ax4" hardware="${hardware}" initial_joint_position="0.0" />
-    <xacro:joint_hardware joint_name="ax5" hardware="${hardware}" initial_joint_position="0.0" />
-    <xacro:joint_hardware joint_name="ax6" hardware="${hardware}" initial_joint_position="0.0" />
+    <xacro:joint_hardware joint_name="ax5" hardware="${hardware}" initial_joint_position="1.0" />
+    <xacro:joint_hardware joint_name="ax6" hardware="${hardware}" initial_joint_position="3.14159" />
+
 
     <xacro:if value="${hardware=='gazebo'}">
       <!-- <xacro:fts link="tool0" name="fts_sensor" tf_prefix="${tf_prefix}"></xacro:fts> -->
+
+      <xacro:rgbd parent="Link1" tf_prefix="${tf_prefix}">
+        <origin rpy="0.0 0.0 -1.57" xyz="0.0 -0.035 0.03"></origin>
+      </xacro:rgbd>
       <gazebo>
         <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
           <parameters>$(arg simulation_controllers)</parameters>
@@ -524,14 +552,21 @@
           </ros>
         </plugin>
         <plugin filename="VacuumGripper" name="rbs_mill_assist::VacuumGripper">
-          <link_name>Link6</link_name>
+          <link_name>Link8</link_name>
         </plugin>
       </gazebo>
     </xacro:if>
-    <gazebo reference="Link6">
-      <sensor name='sensor_contact' type='contact'>
+    <gazebo reference="Link8">
+      <sensor name='left_sensor_contact' type='contact'>
         <contact>
-          <collision>Link6_collision</collision>
+          <collision>Link8_collision</collision>
+        </contact>
+      </sensor>
+    </gazebo>
+    <gazebo reference="Link7">
+      <sensor name='right_sensor_contact' type='contact'>
+        <contact>
+          <collision>Link7_collision</collision>
         </contact>
       </sensor>
     </gazebo>
diff --git a/rbs_mill_assist/urdf/rbs_arm_modular.xacro b/rbs_mill_assist/urdf/rbs_arm_modular.xacro
deleted file mode 100644
index 25c92f4..0000000
--- a/rbs_mill_assist/urdf/rbs_arm_modular.xacro
+++ /dev/null
@@ -1,62 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rbs_arm">
-  <xacro:arg name="gripper_name" default="" />
-  <xacro:arg name="hardware" default="gazebo" />
-  <xacro:arg name="simulation_controllers" default="" />
-  <xacro:arg name="tf_prefix" default="" />
-  <xacro:arg name="namespace" default="arm0"/>
-
-  <xacro:arg name="x" default="-0.10" />
-  <xacro:arg name="y" default="0.0" />
-  <xacro:arg name="z" default="0.03" />
-
-  <xacro:arg name="roll" default="0.0" />
-  <xacro:arg name="pitch" default="0.0" />
-  <xacro:arg name="yaw" default="0.0" />
-
-  <xacro:include filename="$(find rbs_mill_assist)/urdf/rbs_arm_modular_macro.xacro" />
-  <link name="world" />
-
-  <xacro:property name="machine_length" value="0.4"/>
-  <xacro:property name="machine_width" value="0.5"/>
-  <xacro:property name="machine_height" value="0.115"/>
-
-  <link name="machine_link">
-    <visual>
-      <geometry>
-        <box size="${machine_length} ${machine_width} ${machine_height}"/>
-      </geometry>
-      <material name="gray">
-        <color rgba="0 1 0 1"/>
-      </material>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="${machine_length} ${machine_width} ${machine_height}"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="40.0"/>
-      <inertia
-        ixx="${(1/12)*30*(machine_width**2 + machine_height**2)}"
-        iyy="${(1/12)*30*(machine_length**2 + machine_height**2)}"
-        izz="${(1/12)*30*(machine_length**2 + machine_width**2)}"
-        ixy="0"
-        ixz="0"
-        iyz="0"/>
-    </inertial>
-  </link>
-
-  <joint name="machine_joint" type="fixed">
-    <parent link="world"/>
-    <child link="machine_link"/>
-    <origin xyz="0.25 0 ${0.115/2}" rpy="0 0 0"/>
-  </joint>
-
-
-  <!-- Arm-->
-  <xacro:rbs_arm namespace="$(arg namespace)" parent="world" tf_prefix="$(arg tf_prefix)" hardware="$(arg hardware)"
-    gripper_name="$(arg gripper_name)" controllers="$(arg simulation_controllers)">
-    <origin xyz="$(arg x) $(arg y) $(arg z)" rpy="$(arg roll) $(arg pitch) $(arg yaw)" />
-  </xacro:rbs_arm>
-</robot>
diff --git a/rbs_mill_assist/urdf/rbs_arm_modular_macro.xacro b/rbs_mill_assist/urdf/rbs_arm_modular_macro.xacro
deleted file mode 100644
index 2d91c3a..0000000
--- a/rbs_mill_assist/urdf/rbs_arm_modular_macro.xacro
+++ /dev/null
@@ -1,63 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-  <xacro:macro name="rbs_arm" params="namespace parent *origin tf_prefix hardware gripper_name controllers">
-    <xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_base_link.xacro"></xacro:include>
-    <xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_ee_link.xacro"></xacro:include>
-    <xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_fork_link.xacro"></xacro:include>
-    <xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_main_link.xacro"></xacro:include>
-    <xacro:include filename="$(find rbs_arm)/urdf/sensors.xacro"></xacro:include>
-    <!-- BEGIN robot description  -->
-    <!-- link 0-->
-    <xacro:base_link hardware="${hardware}" parent="${parent}" tf_prefix="${tf_prefix}">
-      <xacro:insert_block name="origin"></xacro:insert_block>
-    </xacro:base_link>
-
-    <!-- link 1-->
-    <xacro:fork_link d="0.2" hardware="${hardware}" name="fork0_link" p="10000" parent="base_link" tf_prefix="${tf_prefix}" initial_joint_position="1.57"></xacro:fork_link>
-    <!-- link 2-->
-    <xacro:main_link d="0.2" hardware="${hardware}" name="main0_link" p="10000" parent="fork0_link" tf_prefix="${tf_prefix}" initial_joint_position="0.5"></xacro:main_link>
-    <!-- link 3-->
-    <xacro:fork_link d="0.2" hardware="${hardware}" name="fork1_link" p="10000" parent="main0_link" tf_prefix="${tf_prefix}" initial_joint_position="0.0"></xacro:fork_link>
-    <!-- link 4-->
-    <xacro:main_link d="0.2" hardware="${hardware}" name="main1_link" p="1000" parent="fork1_link" tf_prefix="${tf_prefix}" initial_joint_position="1.5"></xacro:main_link>
-    <!-- ee link also contain tool0-->
-    <!-- link 5-->
-    <xacro:ee_link d="0.2" hardware="${hardware}" p="20" parent="main1_link" tf_prefix="${tf_prefix}"></xacro:ee_link>
-    <!-- END robot description -->
-    <xacro:rgbd parent="${tf_prefix}ee_link" tf_prefix="${tf_prefix}">
-      <origin rpy="0.0 -1.57 0.0" xyz="0.0 0.0 0.0754"></origin>
-    </xacro:rgbd>
-
-    <!-- Insert gripper in robot description if exist -->
-    <!-- <xacro:if value="${gripper_name=='rbs_gripper'}"> -->
-    <!--   <xacro:include filename="$(find rbs_gripper)/urdf/rbs_gripper_macro.xacro"></xacro:include> -->
-    <!--   <xacro:rbs_gripper gripper_name="rbs_gripper" hardware="${hardware}"
-    parent="${tf_prefix}tool0" tf_prefix="${tf_prefix}"> -->
-    <!--     <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"></origin> -->
-    <!--   </xacro:rbs_gripper> -->
-    <!-- </xacro:if> -->
-
-    <!-- If hardware == gazebo so insert additional parameters for it -->
-    <xacro:if value="${hardware=='gazebo'}">
-      <!-- <xacro:fts link="tool0" name="fts_sensor" tf_prefix="${tf_prefix}"></xacro:fts> -->
-      <gazebo>
-        <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
-          <parameters>$(arg simulation_controllers)</parameters>
-          <ros>
-            <namespace>${namespace}</namespace>
-          </ros>
-        </plugin>
-        <plugin filename="VacuumGripper" name="rbs_mill_assist::VacuumGripper">
-          <link_name>ee_link</link_name>
-        </plugin>
-      </gazebo>
-    </xacro:if>
-    <gazebo reference="ee_link">
-      <sensor name='sensor_contact' type='contact'>
-          <contact>
-              <collision>ee_link_collision</collision>
-          </contact>
-      </sensor>
-    </gazebo>
-  </xacro:macro>
-</robot>