diff --git a/rrrobot_src/world/rrrobot_setpoint.world b/rrrobot_src/world/rrrobot_setpoint.world
index ba4b4e6..894ea09 100644
--- a/rrrobot_src/world/rrrobot_setpoint.world
+++ b/rrrobot_src/world/rrrobot_setpoint.world
@@ -991,54 +991,54 @@
       <heading_deg>0</heading_deg>
     </spherical_coordinates>
     <state world_name='default'>
-      <sim_time>84 128000000</sim_time>
-      <real_time>227 16092534</real_time>
-      <wall_time>1586714255 897951104</wall_time>
-      <iterations>84128</iterations>
+      <sim_time>231 66000000</sim_time>
+      <real_time>507 418619758</real_time>
+      <wall_time>1586729050 380379296</wall_time>
+      <iterations>146938</iterations>
       <model name='Fanuc_robotic_arm'>
-        <pose frame=''>0 1e-06 0 2e-06 -0 -3e-06</pose>
+        <pose frame=''>-0 0 0 2e-06 -0 1e-06</pose>
         <scale>1 1 1</scale>
         <link name='base'>
-          <pose frame=''>0 1e-06 0 3.14159 0 -3e-06</pose>
-          <velocity>-2e-06 4e-06 -1.9e-05 5e-06 5.3e-05 3e-06</velocity>
-          <acceleration>-0.014451 -0.003203 -0.002579 0.065768 -0.16485 0.013118</acceleration>
-          <wrench>-1.24396 -0.275747 -0.221966 0 -0 0</wrench>
+          <pose frame=''>-0 0 0 3.14159 -0 1e-06</pose>
+          <velocity>1e-06 1e-06 3e-05 0.000207 0.000211 2e-06</velocity>
+          <acceleration>0.03947 -0.03788 0.086841 0.42064 0.424859 -0.030275</acceleration>
+          <wrench>3.39762 -3.26076 7.47543 0 -0 0</wrench>
         </link>
         <link name='link_1'>
-          <pose frame=''>0.001716 0.003778 0.661683 1.57079 -4e-06 2.3478</pose>
-          <velocity>0.000225 7.9e-05 -9e-06 -0.000224 0.000249 -0.010059</velocity>
-          <acceleration>-0.013679 -0.151361 0.064888 0.206531 0.253364 2.70926</acceleration>
-          <wrench>-3.64238 -40.3028 17.2776 0 -0 0</wrench>
+          <pose frame=''>0.001715 0.003778 0.661683 1.57079 -3e-06 2.36415</pose>
+          <velocity>8.5e-05 -0.000112 3.3e-05 2.3e-05 3e-06 0.004923</velocity>
+          <acceleration>0.162536 -0.194838 0.061524 0.051348 0.004368 -0.035438</acceleration>
+          <wrench>43.2785 -51.8795 16.3821 0 -0 0</wrench>
         </link>
         <link name='link_2'>
-          <pose frame=''>-0.200644 -0.293623 0.474736 -1.32775 -4e-06 2.3478</pose>
-          <velocity>-0.002409 0.002512 -0.001684 0.001575 -0.001632 -0.010095</velocity>
-          <acceleration>-0.653366 -0.603443 0.055824 0.107976 -0.798416 -0.477122</acceleration>
-          <wrench>-39.7214 -36.6863 3.39379 0 -0 0</wrench>
+          <pose frame=''>-0.184155 -0.285108 0.433232 -1.38854 -3e-06 2.36415</pose>
+          <velocity>0.001079 -0.001381 0.001391 -0.001447 0.001439 0.004906</velocity>
+          <acceleration>0.128328 -0.155308 0.043601 -0.120213 -0.05538 -0.516798</acceleration>
+          <wrench>7.80168 -9.44197 2.65075 0 -0 0</wrench>
         </link>
         <link name='link_3'>
-          <pose frame=''>2.76295 2.71626 3.51127 2.48747 -4e-06 2.3478</pose>
-          <velocity>0.035596 -0.019179 -0.025963 -0.005696 0.005776 -0.009954</velocity>
-          <acceleration>-1.52627 -1.27903 -1.61816 -1.42175 0.410946 1.66071</acceleration>
-          <wrench>-97.0888 -81.3618 -102.934 0 -0 0</wrench>
+          <pose frame=''>-0.892768 -0.907878 4.99997 0.892693 -3e-06 2.36415</pose>
+          <velocity>0.006693 -0.002386 2.9e-05 -0.000168 0.000177 0.004946</velocity>
+          <acceleration>0.1229 -0.112233 0.033854 -0.054886 -0.073619 -0.040518</acceleration>
+          <wrench>7.81792 -7.13938 2.15354 0 -0 0</wrench>
         </link>
         <link name='link_4'>
-          <pose frame=''>2.16384 2.12713 -0.948795 -2.5018 -0.146798 -0.599729</pose>
-          <velocity>0.007014 -0.033224 -0.014559 0.130634 0.135525 -0.254293</velocity>
-          <acceleration>-5.4754 -4.48892 -6.4751 -2.49967 1.34692 -2.78923</acceleration>
-          <wrench>-36.2822 -29.7454 -42.9066 0 -0 0</wrench>
+          <pose frame=''>2.249 2.2843 4.26696 -0.869437 -0.236366 -0.972709</pose>
+          <velocity>-0.009922 0.012659 -0.000147 0.020894 0.020977 0.02889</velocity>
+          <acceleration>0.088476 -0.053332 0.012869 0.310775 0.654396 0.36917</acceleration>
+          <wrench>0.586276 -0.353401 0.085278 0 -0 0</wrench>
         </link>
         <link name='link_5'>
-          <pose frame=''>1.00544 1.7326 4.00541 -0.137797 -0.146802 -0.599723</pose>
-          <velocity>0.601991 -0.359545 0.086432 0.12202 0.142802 -0.257395</velocity>
-          <acceleration>-4.38455 -5.27607 -7.10143 2.86584 -1.22955 -2.98271</acceleration>
-          <wrench>-22.4616 -27.0288 -36.3799 0 -0 0</wrench>
+          <pose frame=''>0.213385 1.35124 5.81982 0.13054 -0.236372 -0.972714</pose>
+          <velocity>0.183431 0.012458 0.044866 -0.006391 0.063695 0.018011</velocity>
+          <acceleration>0.053142 -0.040133 0.005787 0.049406 -0.452819 -0.10883</acceleration>
+          <wrench>0.27224 -0.205595 0.029644 0 -0 0</wrench>
         </link>
         <link name='link_6'>
-          <pose frame=''>0.690541 4.69735 4.01685 0.157576 -0.125335 -2.26271</pose>
-          <velocity>1.32005 -0.295835 0.504983 0.125351 0.142495 -0.244604</velocity>
-          <acceleration>-1.71691 -5.67113 -7.06861 -1.70898 -0.167015 1.74113</acceleration>
-          <wrench>-1.25151 -4.13385 -5.15252 0 -0 0</wrench>
+          <pose frame=''>2.34224 3.22369 6.74147 0.089806 0.254371 3.02349</pose>
+          <velocity>0.139247 0.150276 -0.131728 -0.016258 0.068672 0.057563</velocity>
+          <acceleration>0.007493 -0.024085 -0.00486 -0.112806 -0.330423 0.016884</acceleration>
+          <wrench>0.005462 -0.017556 -0.003543 0 -0 0</wrench>
         </link>
       </model>
       <model name='ground_plane'>
@@ -1052,26 +1052,6 @@
         </link>
       </model>
       <model name='unit_box'>
-        <pose frame=''>1 1 1 0 -0 0</pose>
-        <scale>1 1 1</scale>
-        <link name='link'>
-          <pose frame=''>1 1 1 0 -0 0</pose>
-          <velocity>0 0 0 0 -0 0</velocity>
-          <acceleration>0 0 0 0 -0 0</acceleration>
-          <wrench>0 0 0 0 -0 0</wrench>
-        </link>
-      </model>
-      <model name='unit_box_0'>
-        <pose frame=''>1 1 3 0 -0 0</pose>
-        <scale>1 1 1</scale>
-        <link name='link'>
-          <pose frame=''>1 1 3 0 -0 0</pose>
-          <velocity>0 0 0 0 -0 0</velocity>
-          <acceleration>0 0 0 0 -0 0</acceleration>
-          <wrench>0 0 0 0 -0 0</wrench>
-        </link>
-      </model>
-      <model name='unit_box_1'>
         <pose frame=''>1 -1 1 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='link'>
@@ -1081,7 +1061,27 @@
           <wrench>0 0 0 0 -0 0</wrench>
         </link>
       </model>
-      <model name='unit_box_2'>
+      <model name='unit_box_0'>
+        <pose frame=''>1 1 1 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose frame=''>1 1 1 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='unit_box_1'>
+        <pose frame=''>1 1 3 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose frame=''>1 1 3 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='unit_box_3'>
         <pose frame=''>1 -1 2 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='link'>
@@ -1095,6 +1095,60 @@
         <pose frame=''>0 0 10 0 -0 0</pose>
       </light>
     </state>
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose frame=''>7.32519 -0.011987 1.82138 0 0.031643 2.94021</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+    <model name='unit_box_3'>
+      <link name='link'>
+        <inertial>
+          <mass>0.208095</mass>
+          <inertia>
+            <ixx>0.0180922</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.0180922</iyy>
+            <iyz>0</iyz>
+            <izz>0.0346825</izz>
+          </inertia>
+          <pose frame=''>0 0 0 0 -0 0</pose>
+        </inertial>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <gravity>1</gravity>
+        <visual name='visual'>
+          <geometry>
+            <box>
+              <size>0.05 0.05 0.05</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <name>Gazebo/Grey</name>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+            </script>
+            <shader type='pixel'>
+              <normal_map>__default__</normal_map>
+            </shader>
+            <ambient>1 0 0 1</ambient>
+            <diffuse>0.7 0.7 0.7 1</diffuse>
+            <specular>0.01 0.01 0.01 1</specular>
+            <emissive>0 0 0 1</emissive>
+          </material>
+          <pose frame=''>0 0 0 0 -0 0</pose>
+          <transparency>0</transparency>
+          <cast_shadows>1</cast_shadows>
+        </visual>
+      </link>
+      <static>1</static>
+      <allow_auto_disable>1</allow_auto_disable>
+      <pose frame=''>1 -1 2 0 -0 0</pose>
+    </model>
     <model name='unit_box'>
       <link name='link'>
         <inertial>
@@ -1137,65 +1191,10 @@
           <transparency>0</transparency>
           <cast_shadows>1</cast_shadows>
         </visual>
-        <collision name='collision'>
-          <laser_retro>0</laser_retro>
-          <max_contacts>10</max_contacts>
-          <pose frame=''>0 0 0 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.05 0.05 0.05</size>
-            </box>
-          </geometry>
-          <surface>
-            <friction>
-              <ode>
-                <mu>1</mu>
-                <mu2>1</mu2>
-                <fdir1>0 0 0</fdir1>
-                <slip1>0</slip1>
-                <slip2>0</slip2>
-              </ode>
-              <torsional>
-                <coefficient>1</coefficient>
-                <patch_radius>0</patch_radius>
-                <surface_radius>0</surface_radius>
-                <use_patch_radius>1</use_patch_radius>
-                <ode>
-                  <slip>0</slip>
-                </ode>
-              </torsional>
-            </friction>
-            <bounce>
-              <restitution_coefficient>0</restitution_coefficient>
-              <threshold>1e+06</threshold>
-            </bounce>
-            <contact>
-              <collide_without_contact>0</collide_without_contact>
-              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
-              <collide_bitmask>1</collide_bitmask>
-              <ode>
-                <soft_cfm>0</soft_cfm>
-                <soft_erp>0.2</soft_erp>
-                <kp>1e+13</kp>
-                <kd>1</kd>
-                <max_vel>0.01</max_vel>
-                <min_depth>0</min_depth>
-              </ode>
-              <bullet>
-                <split_impulse>1</split_impulse>
-                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
-                <soft_cfm>0</soft_cfm>
-                <soft_erp>0.2</soft_erp>
-                <kp>1e+13</kp>
-                <kd>1</kd>
-              </bullet>
-            </contact>
-          </surface>
-        </collision>
       </link>
       <static>1</static>
       <allow_auto_disable>1</allow_auto_disable>
-      <pose frame=''>3.17326 0.573933 0 0 -0 0</pose>
+      <pose frame=''>1.69337 0.007339 0 0 -0 0</pose>
     </model>
     <model name='unit_box_0'>
       <link name='link'>
@@ -1239,65 +1238,10 @@
           <transparency>0</transparency>
           <cast_shadows>1</cast_shadows>
         </visual>
-        <collision name='collision'>
-          <laser_retro>0</laser_retro>
-          <max_contacts>10</max_contacts>
-          <pose frame=''>0 0 0 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.05 0.05 0.05</size>
-            </box>
-          </geometry>
-          <surface>
-            <friction>
-              <ode>
-                <mu>1</mu>
-                <mu2>1</mu2>
-                <fdir1>0 0 0</fdir1>
-                <slip1>0</slip1>
-                <slip2>0</slip2>
-              </ode>
-              <torsional>
-                <coefficient>1</coefficient>
-                <patch_radius>0</patch_radius>
-                <surface_radius>0</surface_radius>
-                <use_patch_radius>1</use_patch_radius>
-                <ode>
-                  <slip>0</slip>
-                </ode>
-              </torsional>
-            </friction>
-            <bounce>
-              <restitution_coefficient>0</restitution_coefficient>
-              <threshold>1e+06</threshold>
-            </bounce>
-            <contact>
-              <collide_without_contact>0</collide_without_contact>
-              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
-              <collide_bitmask>1</collide_bitmask>
-              <ode>
-                <soft_cfm>0</soft_cfm>
-                <soft_erp>0.2</soft_erp>
-                <kp>1e+13</kp>
-                <kd>1</kd>
-                <max_vel>0.01</max_vel>
-                <min_depth>0</min_depth>
-              </ode>
-              <bullet>
-                <split_impulse>1</split_impulse>
-                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
-                <soft_cfm>0</soft_cfm>
-                <soft_erp>0.2</soft_erp>
-                <kp>1e+13</kp>
-                <kd>1</kd>
-              </bullet>
-            </contact>
-          </surface>
-        </collision>
       </link>
       <static>1</static>
       <allow_auto_disable>1</allow_auto_disable>
-      <pose frame=''>3.77153 -0.833785 0 0 -0 0</pose>
+      <pose frame=''>1.21098 0.975446 0 0 -0 0</pose>
     </model>
     <model name='unit_box_1'>
       <link name='link'>
@@ -1341,174 +1285,10 @@
           <transparency>0</transparency>
           <cast_shadows>1</cast_shadows>
         </visual>
-        <collision name='collision'>
-          <laser_retro>0</laser_retro>
-          <max_contacts>10</max_contacts>
-          <pose frame=''>0 0 0 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.05 0.05 0.05</size>
-            </box>
-          </geometry>
-          <surface>
-            <friction>
-              <ode>
-                <mu>1</mu>
-                <mu2>1</mu2>
-                <fdir1>0 0 0</fdir1>
-                <slip1>0</slip1>
-                <slip2>0</slip2>
-              </ode>
-              <torsional>
-                <coefficient>1</coefficient>
-                <patch_radius>0</patch_radius>
-                <surface_radius>0</surface_radius>
-                <use_patch_radius>1</use_patch_radius>
-                <ode>
-                  <slip>0</slip>
-                </ode>
-              </torsional>
-            </friction>
-            <bounce>
-              <restitution_coefficient>0</restitution_coefficient>
-              <threshold>1e+06</threshold>
-            </bounce>
-            <contact>
-              <collide_without_contact>0</collide_without_contact>
-              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
-              <collide_bitmask>1</collide_bitmask>
-              <ode>
-                <soft_cfm>0</soft_cfm>
-                <soft_erp>0.2</soft_erp>
-                <kp>1e+13</kp>
-                <kd>1</kd>
-                <max_vel>0.01</max_vel>
-                <min_depth>0</min_depth>
-              </ode>
-              <bullet>
-                <split_impulse>1</split_impulse>
-                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
-                <soft_cfm>0</soft_cfm>
-                <soft_erp>0.2</soft_erp>
-                <kp>1e+13</kp>
-                <kd>1</kd>
-              </bullet>
-            </contact>
-          </surface>
-        </collision>
       </link>
       <static>1</static>
       <allow_auto_disable>1</allow_auto_disable>
-      <pose frame=''>3.68535 0.374919 0 0 -0 0</pose>
+      <pose frame=''>1.63258 0.852545 0 0 -0 0</pose>
     </model>
-    <model name='unit_box_2'>
-      <link name='link'>
-        <inertial>
-          <mass>0.208095</mass>
-          <inertia>
-            <ixx>0.0180922</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.0180922</iyy>
-            <iyz>0</iyz>
-            <izz>0.0346825</izz>
-          </inertia>
-          <pose frame=''>0 0 0 0 -0 0</pose>
-        </inertial>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-        <pose frame=''>0 0 0 0 -0 0</pose>
-        <gravity>1</gravity>
-        <visual name='visual'>
-          <geometry>
-            <box>
-              <size>0.05 0.05 0.05</size>
-            </box>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Grey</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-            <shader type='pixel'>
-              <normal_map>__default__</normal_map>
-            </shader>
-            <ambient>1 0 0 1</ambient>
-            <diffuse>0.7 0.7 0.7 1</diffuse>
-            <specular>0.01 0.01 0.01 1</specular>
-            <emissive>0 0 0 1</emissive>
-          </material>
-          <pose frame=''>0 0 0 0 -0 0</pose>
-          <transparency>0</transparency>
-          <cast_shadows>1</cast_shadows>
-        </visual>
-        <collision name='collision'>
-          <laser_retro>0</laser_retro>
-          <max_contacts>10</max_contacts>
-          <pose frame=''>0 0 0 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.05 0.05 0.05</size>
-            </box>
-          </geometry>
-          <surface>
-            <friction>
-              <ode>
-                <mu>1</mu>
-                <mu2>1</mu2>
-                <fdir1>0 0 0</fdir1>
-                <slip1>0</slip1>
-                <slip2>0</slip2>
-              </ode>
-              <torsional>
-                <coefficient>1</coefficient>
-                <patch_radius>0</patch_radius>
-                <surface_radius>0</surface_radius>
-                <use_patch_radius>1</use_patch_radius>
-                <ode>
-                  <slip>0</slip>
-                </ode>
-              </torsional>
-            </friction>
-            <bounce>
-              <restitution_coefficient>0</restitution_coefficient>
-              <threshold>1e+06</threshold>
-            </bounce>
-            <contact>
-              <collide_without_contact>0</collide_without_contact>
-              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
-              <collide_bitmask>1</collide_bitmask>
-              <ode>
-                <soft_cfm>0</soft_cfm>
-                <soft_erp>0.2</soft_erp>
-                <kp>1e+13</kp>
-                <kd>1</kd>
-                <max_vel>0.01</max_vel>
-                <min_depth>0</min_depth>
-              </ode>
-              <bullet>
-                <split_impulse>1</split_impulse>
-                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
-                <soft_cfm>0</soft_cfm>
-                <soft_erp>0.2</soft_erp>
-                <kp>1e+13</kp>
-                <kd>1</kd>
-              </bullet>
-            </contact>
-          </surface>
-        </collision>
-      </link>
-      <static>1</static>
-      <allow_auto_disable>1</allow_auto_disable>
-      <pose frame=''>3.7805 -0.448301 0 0 -0 0</pose>
-    </model>
-    <gui fullscreen='0'>
-      <camera name='user_camera'>
-        <pose frame=''>6.87718 0.050797 2.34784 0 0.147643 2.9762</pose>
-        <view_controller>orbit</view_controller>
-        <projection_type>perspective</projection_type>
-      </camera>
-    </gui>
   </world>
 </sdf>