diff --git a/urdf/camera.urdf.xacro b/urdf/camera.urdf.xacro
new file mode 100644
index 0000000..9d47e25
--- /dev/null
+++ b/urdf/camera.urdf.xacro
@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="camera">
+  
+    <xacro:property name="pi" value="3.1415926535897931"/>
+
+        
+    <xacro:macro name="camera_sensor" params="xyz rpy parent">
+        <joint name="camera_sensor_joint" type="fixed">
+            <axis xyz="0 1 0" />
+            <origin xyz="${xyz}" rpy="${rpy}"/>
+            <parent link="${parent}"/>
+            <child link="camera_link"/>
+        </joint>
+        
+        <link name="camera_link">
+            <collision>
+                <origin xyz="0 0 0" rpy="0 0 0"/>
+                <geometry>
+                    <box size="0.02 0.08 0.05"/>
+                </geometry>
+            </collision>
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0"/>
+                <geometry>
+                    <box size="0.02 0.08 0.05"/>
+                </geometry>
+                <material name="iRobot/Green"/>
+            </visual>
+            <inertial>
+                <mass value="0.0001" />
+                <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+                <inertia ixx="0.0000001" ixy="0" ixz="0" iyy="0.0000001" iyz="0" izz="0.0000001" />
+            </inertial>
+        </link>
+        
+        
+        
+        <gazebo reference="camera_link">
+            <sensor type="camera" name="camera">
+                <update_rate>30.0</update_rate>
+                <camera name="head">
+                    <horizontal_fov>1.3962634</horizontal_fov>
+                    <image>
+                        <width>800</width>
+                        <height>600</height>
+                        <format>R8G8B8</format>
+                    </image>
+                    <clip>
+                        <near>0.02</near>
+                        <far>300</far>
+                    </clip>
+                    <noise>
+                        <type>gaussian</type>
+                        <mean>0.0</mean>
+                        <stddev>0.007</stddev>
+                    </noise>
+                </camera>
+                <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
+                    <alwaysOn>true</alwaysOn>
+                    <updateRate>0.0</updateRate>
+                    <cameraName>camera</cameraName>
+                    <imageTopicName>image_raw</imageTopicName>
+                    <cameraInfoTopicName>camera_info</cameraInfoTopicName>
+                    <frameName>camera_link</frameName>
+                    <hackBaseline>0.07</hackBaseline>
+                    <distortionK1>0.0</distortionK1>
+                    <distortionK2>0.0</distortionK2>
+                    <distortionK3>0.0</distortionK3>
+                    <distortionT1>0.0</distortionT1>
+                    <distortionT2>0.0</distortionT2>
+                </plugin>
+            </sensor>
+    </gazebo>
+    
+    </xacro:macro>
+    
+</robot>
diff --git a/urdf/open_manipulator_cam.urdf.xacro b/urdf/open_manipulator_cam.urdf.xacro
new file mode 100644
index 0000000..3bf55f7
--- /dev/null
+++ b/urdf/open_manipulator_cam.urdf.xacro
@@ -0,0 +1,380 @@
+<?xml version="1.0"?>
+<!-- Open_Manipulator Chain -->
+<robot name="open_manipulator" xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!-- Import all Gazebo-customization elements, including Gazebo colors -->
+  <xacro:include filename="$(find open_manipulator_description)/urdf/open_manipulator.gazebo.xacro" />
+  <!-- Import Rviz colors -->
+  <xacro:include filename="$(find open_manipulator_description)/urdf/materials.xacro" />
+  <xacro:include filename="$(find open_manipulator_description)/urdf/camera.urdf.xacro"/>
+
+
+  <!-- Transmission macro -->
+  <xacro:macro name="SimpleTransmission" params="joint n">
+    <transmission name="tran${n}">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${joint}">
+        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="motor${n}">
+        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+  </xacro:macro>
+
+  <!-- World -->
+  <link name="world">
+  </link>
+
+  <!-- World fixed joint-->
+  <joint name="world_fixed" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="world"/>
+    <child link="link1"/>
+  </joint>
+
+  <!-- Link 1 -->
+  <link name="link1">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://open_manipulator_description/meshes/chain_link1.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://open_manipulator_description/meshes/chain_link1.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+
+<!--    <inertial>
+      <origin xyz="0 0 0" />
+      <mass value="0.082" />
+      <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+               iyy="0.1" iyz="0.0"
+               izz="0.1" />
+    </inertial>-->
+
+     <inertial>
+      <origin xyz="3.0876154e-04 0.0000000e+00 -1.2176461e-04" />
+      <mass value="7.9119962e-02" />
+      <inertia ixx="1.2505234e-05" ixy="0.0" ixz="-1.7855208e-07"
+               iyy="2.1898364e-05" iyz="0.0"
+               izz="1.9267361e-05" />
+    </inertial>
+  </link>
+
+  <!-- Joint 1 -->
+  <joint name="joint1" type="revolute">
+    <parent link="link1"/>
+    <child link="link2"/>
+    <origin xyz="0.012 0.0 0.017" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit velocity="4.8" effort="1" lower="${-pi*0.9}" upper="${pi*0.9}" />
+  </joint>
+
+  <!-- Transmission 1 -->
+  <xacro:SimpleTransmission n="1" joint="joint1" />
+
+  <!--  Link 2 -->
+  <link name="link2">
+    <visual>
+      <origin xyz="0 0 0.019" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://open_manipulator_description/meshes/chain_link2.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0.019" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://open_manipulator_description/meshes/chain_link2.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+
+<!--    <inertial>
+      <origin xyz="0 0 0" />
+      <mass value="0.098" />
+      <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+               iyy="0.1" iyz="0.0"
+               izz="0.1" />
+    </inertial>-->
+
+     <inertial>
+      <origin xyz="-3.0184870e-04 5.4043684e-04 ${0.018 + 2.9433464e-02}" />
+      <mass value="9.8406837e-02" />
+      <inertia ixx="3.4543422e-05" ixy="-1.6031095e-08" ixz="-3.8375155e-07"
+               iyy="3.2689329e-05" iyz="2.8511935e-08"
+               izz="1.8850320e-05" />
+    </inertial>
+  </link>
+
+  <!--  Joint 2 -->
+  <joint name="joint2" type="revolute">
+    <parent link="link2"/>
+    <child link="link3"/>
+    <origin xyz="0.0 0.0 0.0595" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <limit velocity="4.8" effort="1" lower="${-pi*0.57}" upper="${pi*0.5}" />
+  </joint>
+
+  <!-- Transmission 2 -->
+  <xacro:SimpleTransmission n="2" joint="joint2" />
+
+  <!--  Link 3 -->
+  <link name="link3">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://open_manipulator_description/meshes/chain_link3.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://open_manipulator_description/meshes/chain_link3.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+
+<!--    <inertial>
+      <origin xyz="0 0 0" />
+      <mass value="0.136" />
+      <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+               iyy="0.1" iyz="0.0"
+               izz="0.1" />
+    </inertial>-->
+
+     <inertial>
+      <origin xyz="1.0308393e-02 3.7743363e-04 1.0170197e-01" />
+      <mass value="1.3850917e-01" />
+      <inertia ixx="3.3055381e-04" ixy="-9.7940978e-08" ixz="-3.8505711e-05"
+               iyy="3.4290447e-04" iyz="-1.5717516e-06"
+               izz="6.0346498e-05" />
+    </inertial>
+  </link>
+
+  <!--  Joint 3 -->
+  <joint name="joint3" type="revolute">
+    <parent link="link3"/>
+    <child link="link4"/>
+    <origin xyz="0.024 0 0.128" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <limit velocity="4.8" effort="1" lower="${-pi*0.3}" upper="${pi*0.44}" />
+  </joint>
+
+  <!-- Transmission 3 -->
+  <xacro:SimpleTransmission n="3" joint="joint3" />
+
+  <!--  Link 4 -->
+  <link name="link4">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://open_manipulator_description/meshes/chain_link4.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://open_manipulator_description/meshes/chain_link4.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+
+<!--    <inertial>
+      <origin xyz="0 0 0" />
+      <mass value="0.131" />
+      <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+               iyy="0.1" iyz="0.0"
+               izz="0.1" />
+    </inertial>-->
+
+     <inertial>
+      <origin xyz="9.0909590e-02 3.8929816e-04 2.2413279e-04" />
+      <mass value="1.3274562e-01" />
+      <inertia ixx="3.0654178e-05" ixy="-1.2764155e-06" ixz="-2.6874417e-07"
+               iyy="2.4230292e-04" iyz="1.1559550e-08"
+               izz="2.5155057e-04" />
+    </inertial>
+  </link>
+
+  <!--  Joint 4 -->
+  <joint name="joint4" type="revolute">
+    <parent link="link4"/>
+    <child link="link5"/>
+    <origin xyz="0.124 0.0 0.0" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <limit velocity="4.8" effort="1" lower="${-pi*0.57}" upper="${pi*0.65}" />
+  </joint>
+
+  <!-- Transmission 4 -->
+  <xacro:SimpleTransmission n="4" joint="joint4" />
+
+  <!--  Link 5 -->
+  <link name="link5">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://open_manipulator_description/meshes/chain_link5.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://open_manipulator_description/meshes/chain_link5.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+
+<!--    <inertial>
+      <origin xyz="0 0 0" />
+      <mass value="0.141" />
+      <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+               iyy="0.1" iyz="0.0"
+               izz="0.1" />
+    </inertial>-->
+
+     <inertial>
+      <origin xyz="4.4206755e-02 3.6839985e-07 8.9142216e-03" />
+      <mass value="1.4327573e-01" />
+      <inertia ixx="8.0870749e-05" ixy="0.0" ixz="-1.0157896e-06"
+               iyy="7.5980465e-05" iyz="0.0"
+               izz="9.3127351e-05" />
+    </inertial>
+  </link>
+
+  <!--  Gripper link -->
+  <link name="gripper_link">
+    <visual>
+      <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_l.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+
+    <collision>
+      <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_l.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+
+    <inertial>
+      <origin xyz="0 0 0" />
+      <mass value="0.017" />
+      <inertia ixx="1.0e-03" ixy="0.0" ixz="0.0"
+               iyy="1.0e-03" iyz="0.0"
+               izz="1.0e-03" />
+    </inertial>
+
+<!--     <inertial>
+      <origin xyz="${0.028 + 8.3720668e-03} ${0.0246 + 9.9696160e-03} -4.2836895e-07" />
+      <mass value="3.2218127e-02" />
+      <inertia ixx="9.5568826e-06" ixy="2.8424644e-06" ixz="-3.2829197e-10"
+               iyy="2.2552871e-05" iyz="-3.1463634e-10"
+               izz="1.7605306e-05" />
+    </inertial>-->
+  </link>
+
+  <!--  Gripper joint -->
+  <joint name="gripper" type="prismatic">
+    <parent link="link5"/>
+    <child link="gripper_link"/>
+    <origin xyz="0.0817 0.021 0.0" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <limit velocity="4.8" effort="1" lower="-0.010" upper="0.019" />
+  </joint>
+
+  <!-- Transmission 5 -->
+  <xacro:SimpleTransmission n="5" joint="gripper" />
+
+  <!--  Gripper link sub -->
+  <link name="gripper_link_sub">
+    <visual>
+      <origin xyz="0.0 -0.0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_r.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+
+    <collision>
+      <origin xyz="0.0 -0.0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_r.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+
+    <inertial>
+      <origin xyz="0 0 0" />
+      <mass value="0.017" />
+      <inertia ixx="1.0e-03" ixy="0.0" ixz="0.0"
+               iyy="1.0e-03" iyz="0.0"
+               izz="1.0e-03" />
+    </inertial>
+
+<!--     <inertial>
+      <origin xyz="${0.028 + 8.3720668e-03} ${-0.0246 - 9.9696160e-03} -4.2836895e-07" />
+      <mass value="3.2218127e-02" />
+      <inertia ixx="9.5568826e-06" ixy="2.8424644e-06" ixz="-3.2829197e-10"
+               iyy="2.2552871e-05" iyz="-3.1463634e-10"
+               izz="1.7605306e-05" />
+    </inertial>-->
+  </link>
+
+  <!--  Gripper joint sub -->
+  <joint name="gripper_sub" type="prismatic">
+    <parent link="link5"/>
+    <child link="gripper_link_sub"/>
+    <origin xyz="0.0817 -0.021 0" rpy="0 0 0"/>
+    <axis xyz="0 -1 0"/>
+    <limit velocity="4.8" effort="1" lower="-0.010" upper="0.019" />
+    <mimic joint="gripper" multiplier="1"/>
+  </joint>
+
+  <!-- Transmission 6 -->
+  <xacro:SimpleTransmission n="6" joint="gripper_sub" />
+
+  <!-- end effector joint -->
+  <joint name="end_effector_joint" type="fixed">
+    <origin xyz="0.126 0.0 0.0" rpy="0 0 0"/>
+    <parent link="link5"/>
+    <child link="end_effector_link"/>
+  </joint>
+
+  <!-- end effector link -->
+  <link name="end_effector_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01" />
+      </geometry>
+      <material name="red"/>
+     </visual>
+
+    <inertial>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <mass value="0.001"/>
+      <inertia ixx="1.0e-06" ixy="0.0" ixz="0.0" iyy="1.0e-06" iyz="0.0" izz="1.0e-06" />
+    </inertial>
+  </link>
+
+    <!--Camera link and plugin-->
+    <xacro:camera_sensor xyz="0.2 0 ${0.4 + 0.2}" 
+                  rpy="0 1.57 0"
+                  parent="world">
+    </xacro:camera_sensor>
+
+
+
+</robot>
diff --git a/urdf/open_manipulator_with_cam.urdf.xacro b/urdf/open_manipulator_with_cam.urdf.xacro
new file mode 100755
index 0000000..5e2715f
--- /dev/null
+++ b/urdf/open_manipulator_with_cam.urdf.xacro
@@ -0,0 +1,316 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="open_manipulator">
+
+  <xacro:include filename="$(find kair_algorithms)/urdf/camera.urdf.xacro"/>
+    <material name="black">
+      <color rgba="0.0 0.0 0.0 1.0"/>
+    </material>
+    <material name="white">
+      <color rgba="1.0 1.0 1.0 1.0"/>
+    </material>
+    <material name="red">
+      <color rgba="0.8 0.0 0.0 1.0"/>
+    </material>
+    <material name="blue">
+      <color rgba="0.0 0.0 0.8 1.0"/>
+    </material>
+    <material name="green">
+      <color rgba="0.0 0.8 0.0 1.0"/>
+    </material>
+    <material name="grey">
+      <color rgba="0.5 0.5 0.5 1.0"/>
+    </material>
+    <material name="orange">
+      <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
+    </material>
+    <material name="brown">
+      <color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
+    </material>
+    <!-- World -->
+    <link name="world">
+    </link>
+    <!-- World fixed joint-->
+    <joint name="world_fixed" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <parent link="world"/>
+      <child link="link1"/>
+    </joint>
+    <!-- Link 1 -->
+    <link name="link1">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link1.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+        <material name="grey"/>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link1.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+      </collision>
+      <!--    <inertial>
+        <origin xyz="0 0 0" />
+        <mass value="0.082" />
+        <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+                 iyy="0.1" iyz="0.0"
+                 izz="0.1" />
+      </inertial>-->
+      <inertial>
+        <origin xyz="3.0876154e-04 0.0000000e+00 -1.2176461e-04"/>
+        <mass value="7.9119962e-02"/>
+        <inertia ixx="1.2505234e-05" ixy="0.0" ixz="-1.7855208e-07" iyy="2.1898364e-05" iyz="0.0" izz="1.9267361e-05"/>
+      </inertial>
+    </link>
+    <!-- Joint 1 -->
+    <joint name="joint1" type="revolute">
+      <parent link="link1"/>
+      <child link="link2"/>
+      <origin rpy="0 0 0" xyz="0.012 0.0 0.017"/>
+      <axis xyz="0 0 1"/>
+      <limit effort="1" lower="-2.82743338823" upper="2.82743338823" velocity="4.8"/>
+    </joint>
+    <!--  Link 2 -->
+    <link name="link2">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0.019"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link2.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+        <material name="grey"/>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0.019"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link2.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+      </collision>
+      <!--    <inertial>
+        <origin xyz="0 0 0" />
+        <mass value="0.098" />
+        <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+                 iyy="0.1" iyz="0.0"
+                 izz="0.1" />
+      </inertial>-->
+      <inertial>
+        <origin xyz="-3.0184870e-04 5.4043684e-04 0.047433464"/>
+        <mass value="9.8406837e-02"/>
+        <inertia ixx="3.4543422e-05" ixy="-1.6031095e-08" ixz="-3.8375155e-07" iyy="3.2689329e-05" iyz="2.8511935e-08" izz="1.8850320e-05"/>
+      </inertial>
+    </link>
+    <!--  Joint 2 -->
+    <joint name="joint2" type="revolute">
+      <parent link="link2"/>
+      <child link="link3"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0595"/>
+      <axis xyz="0 1 0"/>
+      <limit effort="1" lower="-1.79070781255" upper="1.57079632679" velocity="4.8"/>
+    </joint>
+    <!--  Link 3 -->
+    <link name="link3">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link3.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+        <material name="grey"/>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link3.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+      </collision>
+      <!--    <inertial>
+        <origin xyz="0 0 0" />
+        <mass value="0.136" />
+        <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+                 iyy="0.1" iyz="0.0"
+                 izz="0.1" />
+      </inertial>-->
+      <inertial>
+        <origin xyz="1.0308393e-02 3.7743363e-04 1.0170197e-01"/>
+        <mass value="1.3850917e-01"/>
+        <inertia ixx="3.3055381e-04" ixy="-9.7940978e-08" ixz="-3.8505711e-05" iyy="3.4290447e-04" iyz="-1.5717516e-06" izz="6.0346498e-05"/>
+      </inertial>
+    </link>
+    <!--  Joint 3 -->
+    <joint name="joint3" type="revolute">
+      <parent link="link3"/>
+      <child link="link4"/>
+      <origin rpy="0 0 0" xyz="0.024 0 0.128"/>
+      <axis xyz="0 1 0"/>
+      <limit effort="1" lower="-0.942477796077" upper="1.38230076758" velocity="4.8"/>
+    </joint>
+
+    <!--  Link 4 -->
+    <link name="link4">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link4.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+        <material name="grey"/>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link4.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+      </collision>
+      <!--    <inertial>
+        <origin xyz="0 0 0" />
+        <mass value="0.131" />
+        <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+                 iyy="0.1" iyz="0.0"
+                 izz="0.1" />
+      </inertial>-->
+      <inertial>
+        <origin xyz="9.0909590e-02 3.8929816e-04 2.2413279e-04"/>
+        <mass value="1.3274562e-01"/>
+        <inertia ixx="3.0654178e-05" ixy="-1.2764155e-06" ixz="-2.6874417e-07" iyy="2.4230292e-04" iyz="1.1559550e-08" izz="2.5155057e-04"/>
+      </inertial>
+    </link>
+    <!--  Joint 4 -->
+    <joint name="joint4" type="revolute">
+      <parent link="link4"/>
+      <child link="link5"/>
+      <origin rpy="0 0 0" xyz="0.124 0.0 0.0"/>
+      <axis xyz="0 1 0"/>
+      <limit effort="1" lower="-1.79070781255" upper="2.04203522483" velocity="4.8"/>
+    </joint>
+
+    <!--  Link 5 -->
+    <link name="link5">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link5.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+        <material name="grey"/>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link5.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+      </collision>
+      <!--    <inertial>
+        <origin xyz="0 0 0" />
+        <mass value="0.141" />
+        <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+                 iyy="0.1" iyz="0.0"
+                 izz="0.1" />
+      </inertial>-->
+      <inertial>
+        <origin xyz="4.4206755e-02 3.6839985e-07 8.9142216e-03"/>
+        <mass value="1.4327573e-01"/>
+        <inertia ixx="8.0870749e-05" ixy="0.0" ixz="-1.0157896e-06" iyy="7.5980465e-05" iyz="0.0" izz="9.3127351e-05"/>
+      </inertial>
+    </link>
+    <!--  Gripper link -->
+    <link name="gripper_link">
+      <visual>
+        <origin rpy="0 0 0" xyz="0.0 0.0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_l.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+        <material name="grey"/>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.0 0.0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_l.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <origin xyz="0 0 0"/>
+        <mass value="0.017"/>
+        <inertia ixx="1.0e-03" ixy="0.0" ixz="0.0" iyy="1.0e-03" iyz="0.0" izz="1.0e-03"/>
+      </inertial>
+      <!--     <inertial>
+        <origin xyz="${0.028 + 8.3720668e-03} ${0.0246 + 9.9696160e-03} -4.2836895e-07" />
+        <mass value="3.2218127e-02" />
+        <inertia ixx="9.5568826e-06" ixy="2.8424644e-06" ixz="-3.2829197e-10"
+                 iyy="2.2552871e-05" iyz="-3.1463634e-10"
+                 izz="1.7605306e-05" />
+      </inertial>-->
+    </link>
+    <!--  Gripper joint -->
+    <joint name="gripper" type="prismatic">
+      <parent link="link5"/>
+      <child link="gripper_link"/>
+      <origin rpy="0 0 0" xyz="0.0817 0.021 0.0"/>
+      <axis xyz="0 1 0"/>
+      <limit effort="1" lower="-0.010" upper="0.019" velocity="4.8"/>
+    </joint>
+
+    <!--  Gripper link sub -->
+    <link name="gripper_link_sub">
+      <visual>
+        <origin rpy="0 0 0" xyz="0.0 -0.0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_r.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+        <material name="grey"/>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.0 -0.0 0"/>
+        <geometry>
+          <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_r.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <origin xyz="0 0 0"/>
+        <mass value="0.017"/>
+        <inertia ixx="1.0e-03" ixy="0.0" ixz="0.0" iyy="1.0e-03" iyz="0.0" izz="1.0e-03"/>
+      </inertial>
+      <!--     <inertial>
+        <origin xyz="${0.028 + 8.3720668e-03} ${-0.0246 - 9.9696160e-03} -4.2836895e-07" />
+        <mass value="3.2218127e-02" />
+        <inertia ixx="9.5568826e-06" ixy="2.8424644e-06" ixz="-3.2829197e-10"
+                 iyy="2.2552871e-05" iyz="-3.1463634e-10"
+                 izz="1.7605306e-05" />
+      </inertial>-->
+    </link>
+    <!--  Gripper joint sub -->
+    <joint name="gripper_sub" type="prismatic">
+      <parent link="link5"/>
+      <child link="gripper_link_sub"/>
+      <origin rpy="0 0 0" xyz="0.0817 -0.021 0"/>
+      <axis xyz="0 -1 0"/>
+      <limit effort="1" lower="-0.010" upper="0.019" velocity="4.8"/>
+      <mimic joint="gripper" multiplier="1"/>
+    </joint>
+
+    <!-- end effector joint -->
+    <joint name="end_effector_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="0.126 0.0 0.0"/>
+      <parent link="link5"/>
+      <child link="end_effector_link"/>
+    </joint>
+    <!-- end effector link -->
+    <link name="end_effector_link">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <box size="0.01 0.01 0.01"/>
+        </geometry>
+        <material name="red"/>
+      </visual>
+      <inertial>
+        <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+        <mass value="0.001"/>
+        <inertia ixx="1.0e-06" ixy="0.0" ixz="0.0" iyy="1.0e-06" iyz="0.0" izz="1.0e-06"/>
+      </inertial>
+    </link>
+
+
+    <!--Camera link and plugin-->
+    <xacro:camera_sensor xyz="2 0 ${0.04 + 0.02 + 0.04}" 
+                  rpy="0 0 0"
+                  parent="world">
+    </xacro:camera_sensor>
+
+  </robot>