diff --git a/rasmt_moveit_config/config/rasmt.srdf b/rasmt_moveit_config/config/rasmt.srdf
index 98cf235..852afe7 100644
--- a/rasmt_moveit_config/config/rasmt.srdf
+++ b/rasmt_moveit_config/config/rasmt.srdf
@@ -10,7 +10,7 @@
     <!--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="rasmt_arm_group">
-        <chain base_link="rasmt_Base_Link" tip_link="rasmt_Dock_Link"/>
+        <chain base_link="rasmt_Base_Link" tip_link="rasmt_tool_end_point"/>
     </group>
     <group name="rasmt_hand_arm_group">
         <link name="rasmt_Grip_Body"/>
diff --git a/rasmt_moveit_config/config/rasmt_moveit.rviz b/rasmt_moveit_config/config/rasmt_moveit.rviz
index ce7b49f..5198941 100644
--- a/rasmt_moveit_config/config/rasmt_moveit.rviz
+++ b/rasmt_moveit_config/config/rasmt_moveit.rviz
@@ -7,7 +7,7 @@ Panels:
         - /Global Options1
         - /Status1
       Splitter Ratio: 0.5
-    Tree Height: 389
+    Tree Height: 787
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -43,7 +43,7 @@ Visualization Manager:
       Reference Frame: <Fixed Frame>
       Value: true
     - Class: moveit_rviz_plugin/PlanningScene
-      Enabled: false
+      Enabled: true
       Move Group Namespace: ""
       Name: PlanningScene
       Planning Scene Topic: /monitored_planning_scene
@@ -63,13 +63,75 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          rasmt_Base_Link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          rasmt_Dock_Link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          rasmt_Fork_1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          rasmt_Fork_2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          rasmt_Fork_3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          rasmt_Grip_Body:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          rasmt_Grip_L:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          rasmt_Grip_R:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          rasmt_Link_1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          rasmt_Link_2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          rasmt_tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          rasmt_tool_end_point:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: true
-      Value: false
+      Value: true
     - Acceleration_Scaling_Factor: 1
       Class: moveit_rviz_plugin/MotionPlanning
-      Enabled: true
+      Enabled: false
       Move Group Namespace: ""
       MoveIt_Allow_Approximate_IK: false
       MoveIt_Allow_External_Program: false
@@ -150,6 +212,14 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          rasmt_tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          rasmt_tool_end_point:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           world:
             Alpha: 1
             Show Axes: false
@@ -249,6 +319,14 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          rasmt_tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          rasmt_tool_end_point:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           world:
             Alpha: 1
             Show Axes: false
@@ -256,7 +334,7 @@ Visualization Manager:
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: true
-      Value: true
+      Value: false
       Velocity_Scaling_Factor: 1
   Enabled: true
   Global Options:
@@ -308,18 +386,18 @@ Visualization Manager:
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.35200047492980957
-        Y: 0.026240859180688858
-        Z: 0.2903156876564026
+        X: 0.35113412141799927
+        Y: 0.04733692854642868
+        Z: 0.32357215881347656
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5153983235359192
+      Pitch: 0.4853982627391815
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 5.318596363067627
+      Yaw: 5.343599319458008
     Saved: ~
 Window Geometry:
   Displays:
@@ -331,7 +409,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001f30000039efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000210000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000253000001880000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000053f0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f30000039efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000025e0000017d0000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000053f0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:
diff --git a/rasmt_support/urdf/rasmt.xacro b/rasmt_support/urdf/rasmt.xacro
index ed44547..04b5321 100644
--- a/rasmt_support/urdf/rasmt.xacro
+++ b/rasmt_support/urdf/rasmt.xacro
@@ -5,10 +5,14 @@
 <xacro:arg name="sim" default="true"/>
 <xacro:arg name="robot_name" default="rasmt"/>
 
+<xacro:if value="$(arg grip)">
+    <xacro:property name="gripper_length" value="0.12224"/>
+</xacro:if>
+
 <link name="world"/>
 
 <xacro:include filename="$(find rasmt_support)/urdf/robot/rasmt_single_macro.xacro"/>
-<xacro:rasmt_single prefix="$(arg robot_name)" parent="world" sim="$(arg sim)" xyz="0 0 0"/>
+<xacro:rasmt_single prefix="$(arg robot_name)" parent="world" sim="$(arg sim)" xyz="0 0 0" gripper_length="${gripper_length}"/>
 
 <xacro:if value="$(arg grip)">
     <xacro:include filename="$(find rasmt_support)/urdf/tools/rasmt_hand_macro.xacro"/>
diff --git a/rasmt_support/urdf/robot/rasmt_single_macro.xacro b/rasmt_support/urdf/robot/rasmt_single_macro.xacro
index 82e8ccf..a93c6b6 100644
--- a/rasmt_support/urdf/robot/rasmt_single_macro.xacro
+++ b/rasmt_support/urdf/robot/rasmt_single_macro.xacro
@@ -4,7 +4,7 @@
   <xacro:include filename="$(find rasmt_support)/urdf/robot/rasmt_single_control.xacro"/>
   <xacro:include filename="$(find rasmt_support)/urdf/robot/rasmt_single_gazebo.xacro"/>
 
-<xacro:macro name="rasmt_single" params="prefix parent sim xyz">
+<xacro:macro name="rasmt_single" params="prefix parent sim xyz gripper_length">
 
 
   <joint name="to_${parent}" type="fixed">
@@ -401,5 +401,21 @@
   <xacro:rasmt_single_gazebo prefix="${prefix}"/>
   <xacro:rasmt_single_control prefix="${prefix}" sim="${sim}"/>
 
+  <link name="${prefix}_tool0"/>
+  <joint name="${prefix}_tool0_fixed" type="fixed">
+      <parent link="${prefix}_Dock_Link"/>
+      <child link="${prefix}_tool0"/>
+      <origin xyz="0 0 0.12324" rpy="0 0 0"/>
+      <axis xyz="0 0 0"/>
+  </joint>
+
+  <link name="${prefix}_tool_end_point"/>
+  <joint name="${prefix}_tool_end_point_to_tool0" type="fixed">
+      <parent link="${prefix}_tool0"/>
+      <child link="${prefix}_tool_end_point"/>
+      <origin xyz="0 0 ${gripper_length}" rpy="0 0 0"/>
+      <axis xyz="0 0 0"/>
+  </joint>
+
   </xacro:macro>
 </robot>