Add URDF. Some changes in scene.

update MainTree
add twist cmd with condition BT plugin and refactor VacuumGripper to publish contact info
2025-03-19 15:23:57 +03:00 · 2025-03-17 17:32:38 +03:00 · 2025-03-17 14:20:21 +03:00 · 2025-03-17 14:20:21 +03:00 · 2025-03-17 14:20:21 +03:00 · 2025-03-17 14:20:21 +03:00
105 changed files with 6021 additions and 447 deletions
--- a/rbs_mill_assist/CMakeLists.txt
+++ b/rbs_mill_assist/CMakeLists.txt
@ -7,51 +7,13 @@ endif()

 # find dependencies
 find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(std_srvs REQUIRED)
-find_package(gz-cmake3 REQUIRED)
-find_package(gz-plugin2 REQUIRED COMPONENTS register)
-set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
-find_package(gz-common5 REQUIRED COMPONENTS profiler)
-set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR})
-
-# Harmonic
-if("$ENV{GZ_VERSION}" STREQUAL "harmonic")
-  find_package(gz-sim8 REQUIRED)
-  set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
-  message(STATUS "Compiling against Gazebo Harmonic")
-# Default to Garden
-else()
-  find_package(gz-sim7 REQUIRED)
-  set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR})
-  message(STATUS "Compiling against Gazebo Garden")
-endif()
-
-add_library(VacuumGripper
-  SHARED
-  src/VacuumGripper.cpp
-)

 include_directories(include)

-target_link_libraries(VacuumGripper
-  gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
-)
-
-ament_target_dependencies(VacuumGripper
-  rclcpp
-  std_srvs
-)
-
-install(
-  TARGETS VacuumGripper
-  DESTINATION lib/${PROJECT_NAME}
-)
-
-
-install(DIRECTORY world urdf meshes launch config assets bt/xmls bt/config
+install(DIRECTORY world urdf meshes moveit srdf launch config assets bt/xmls bt/config
 DESTINATION share/${PROJECT_NAME})

+add_subdirectory(src)
 add_subdirectory(scripts)
 add_subdirectory(bt)

--- a/rbs_mill_assist/assets/bunker/meshes/Bunker_for_labels_60х40.STL
+++ b/rbs_mill_assist/assets/bunker/meshes/Bunker_for_labels_60х40.STL
--- a/rbs_mill_assist/assets/bunker/model.sdf
+++ b/rbs_mill_assist/assets/bunker/model.sdf
@ -6,41 +6,41 @@
      <visual name="bunker_visual">
        <geometry>
          <mesh>
-            <uri>model://bunker/meshes/bunker.stl</uri>
+            <uri>model://bunker/meshes/Bunker_for_labels_60х40.STL</uri>
          </mesh>
        </geometry>
-        <!-- <material> -->
-        <!--   <diffuse>1 1 1 1</diffuse> -->
-        <!--   <ambient>1 1 1 1</ambient> -->
-        <!--   <specular>0.5 0.5 0.5 1</specular> -->
-        <!--   <emissive>0 0 0 1</emissive> -->
-        <!--   <texture> -->
-        <!--     <diffuse_map>model://bunker/textures/shildik_sh.png</diffuse_map> -->
-        <!--   </texture> -->
-        <!--   <pbr> -->
-        <!--     <metal> -->
-        <!--       <albedo_map>model://bunker/textures/shildik_sh_d.png</albedo_map> -->
-        <!--       <normal_map>model://bunker/textures/shildik_sh_n.png</normal_map> -->
-        <!--       <roughness_map>model://bunker/textures/shildik_sh_r.png</roughness_map> -->
-        <!--       <metalness_map>model://bunker/textures/shildik_sh_m.png</metalness_map> -->
-        <!--       <ambient_occlusion_map>model://bunker/textures/shildik_sh_o.png</ambient_occlusion_map> -->
-        <!--     </metal> -->
-        <!--   </pbr> -->
-        <!-- </material> -->
+        <material>
+          <diffuse>1 1 1 1</diffuse>
+          <ambient>1 1 1 1</ambient>
+          <specular>0.5 0.5 0.5 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
      </visual>
-      <collision name="laser_collision">
+      <collision name="bunker_collision">
        <geometry>
          <mesh>
-            <uri>model://bunker/meshes/bunker.stl</uri>
+            <uri>model://bunker/meshes/Bunker_for_labels_60х40.STL</uri>
          </mesh>
        </geometry>
        <surface>
          <contact>
-            <ode />
+            <ode>
+              <kp>1e6</kp>
+              <kd>1e3</kd>
+              <max_vel>0.1</max_vel>
+              <min_depth>0.001</min_depth>
+            </ode>
          </contact>
-          <bounce />
+          <bounce>
+            <restitution>0.3</restitution>
+          </bounce>
          <friction>
-            <ode />
+            <ode>
+              <mu>0.61</mu>
+              <mu2>0.47</mu2>
+              <slip1>0.0</slip1>
+              <slip2>0.0</slip2>
+            </ode>
          </friction>
        </surface>
      </collision>
--- a/rbs_mill_assist/assets/conductor/meshes/conductor.stl
+++ b/rbs_mill_assist/assets/conductor/meshes/conductor.stl
--- a/rbs_mill_assist/assets/conductor/model.config
+++ b/rbs_mill_assist/assets/conductor/model.config
@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>conductor</name>
+  <version>0.1</version>
+  <sdf version='1.10'>model.sdf</sdf>
+
+  <author>
+    <name>Bill Finger</name>
+    <email>ur.narmak@gmail.com</email>
+  </author>
+
+  <description>
+    Empty
+  </description>
+</model>
--- a/rbs_mill_assist/assets/conductor/model.sdf
+++ b/rbs_mill_assist/assets/conductor/model.sdf
@ -0,0 +1,60 @@
+<?xml version="1.0"?>
+<sdf version="1.10">
+  <model name="conductor">
+    <link name="conductor_link">
+      <inertial auto="true" />
+      <visual name="conductor_visual">
+        <geometry>
+          <mesh>
+            <uri>model://conductor/meshes/conductor.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <diffuse>0.8 0.8 0.8 1</diffuse>
+          <ambient>0.3 0.3 0.3 1</ambient>
+          <specular>0.2 0.2 0.2 1</specular>
+          <emissive>0 0 0 1</emissive>
+          <!-- <texture> -->
+          <!--   <diffuse_map>model://conductor/textures/Metal055A_2K-PNG_Color.png</diffuse_map> -->
+          <!-- </texture> -->
+          <!-- <pbr> -->
+          <!--   <metal> -->
+          <!--     <albedo_map>model://conductor/textures/Metal055A_2K-PNG_Color.png</albedo_map> -->
+          <!--     <normal_map>model://conductor/textures/Metal055A_2K-PNG_NormalDX.png</normal_map> -->
+          <!--     <roughness_map>model://conductor/textures/Metal055A_2K-PNG_Roughness.png</roughness_map> -->
+          <!--     <metalness_map>model://conductor/textures/Metal055A_2K-PNG_Metalness.png</metalness_map> -->
+          <!--   </metal> -->
+          <!-- </pbr> -->
+        </material>
+      </visual>
+      <collision name="conductor_collision">
+        <geometry>
+          <mesh>
+            <uri>model://conductor/meshes/conductor.stl</uri>
+          </mesh>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <kp>1e6</kp>
+              <kd>1e3</kd>
+              <max_vel>0.1</max_vel>
+              <min_depth>0.001</min_depth>
+            </ode>
+          </contact>
+          <bounce>
+            <restitution>0.3</restitution>
+          </bounce>
+          <friction>
+            <ode>
+              <mu>0.61</mu>
+              <mu2>0.47</mu2>
+              <slip1>0.0</slip1>
+              <slip2>0.0</slip2>
+            </ode>
+          </friction>
+        </surface>
+      </collision>
+    </link>
+  </model>
+</sdf>
--- a/rbs_mill_assist/assets/laser/laser.material
+++ b/rbs_mill_assist/assets/laser/laser.material
@ -0,0 +1,11 @@
+material LaserMaterial
+{
+    technique
+    {
+        pass
+        {
+            ambient 0.5 0.5 0.5 1.0
+            diffuse 0.5 0.5 0.5 1.0
+        }
+    }
+}
--- a/rbs_mill_assist/assets/laser/meshes/laser.dae
+++ b/rbs_mill_assist/assets/laser/meshes/laser.dae
@ -0,0 +1,207 @@
+<?xml version="1.0" encoding="utf-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
+  <asset>
+    <contributor>
+      <author>Blender User</author>
+      <authoring_tool>Blender 4.3.2 commit date:2024-12-16, commit time:21:10, hash:32f5fdce0a0a</authoring_tool>
+    </contributor>
+    <created>2025-03-05T17:38:05</created>
+    <modified>2025-03-05T17:38:05</modified>
+    <unit name="meter" meter="1"/>
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_cameras>
+    <camera id="Camera_001-camera" name="Camera.001">
+      <optics>
+        <technique_common>
+          <perspective>
+            <xfov sid="xfov">39.59775</xfov>
+            <aspect_ratio>1.777778</aspect_ratio>
+            <znear sid="znear">0.1</znear>
+            <zfar sid="zfar">100</zfar>
+          </perspective>
+        </technique_common>
+      </optics>
+      <extra>
+        <technique profile="blender">
+          <shiftx sid="shiftx" type="float">0</shiftx>
+          <shifty sid="shifty" type="float">0</shifty>
+          <dof_distance sid="dof_distance" type="float">10</dof_distance>
+        </technique>
+      </extra>
+    </camera>
+    <camera id="Camera-camera" name="Camera">
+      <optics>
+        <technique_common>
+          <perspective>
+            <xfov sid="xfov">39.59775</xfov>
+            <aspect_ratio>1.777778</aspect_ratio>
+            <znear sid="znear">0.1</znear>
+            <zfar sid="zfar">100</zfar>
+          </perspective>
+        </technique_common>
+      </optics>
+      <extra>
+        <technique profile="blender">
+          <shiftx sid="shiftx" type="float">0</shiftx>
+          <shifty sid="shifty" type="float">0</shifty>
+          <dof_distance sid="dof_distance" type="float">10</dof_distance>
+        </technique>
+      </extra>
+    </camera>
+  </library_cameras>
+  <library_lights>
+    <light id="Light_001-light" name="Light.001">
+      <technique_common>
+        <point>
+          <color sid="color">1000 1000 1000</color>
+          <constant_attenuation>1</constant_attenuation>
+          <linear_attenuation>0</linear_attenuation>
+          <quadratic_attenuation>0</quadratic_attenuation>
+        </point>
+      </technique_common>
+      <extra>
+        <technique profile="blender">
+          <type sid="type" type="int">0</type>
+          <flag sid="flag" type="int">0</flag>
+          <mode sid="mode" type="int">2097153</mode>
+          <red sid="red" type="float">1</red>
+          <green sid="green" type="float">1</green>
+          <blue sid="blue" type="float">1</blue>
+          <energy sid="blender_energy" type="float">1000</energy>
+          <spotsize sid="spotsize" type="float">75</spotsize>
+          <spotblend sid="spotblend" type="float">0.15</spotblend>
+          <clipsta sid="clipsta" type="float">0.04999995</clipsta>
+          <clipend sid="clipend" type="float">40</clipend>
+          <radius sid="radius" type="float">0.1</radius>
+          <area_shape sid="area_shape" type="int">1</area_shape>
+          <area_size sid="area_size" type="float">0.1</area_size>
+          <area_sizey sid="area_sizey" type="float">0.1</area_sizey>
+          <area_sizez sid="area_sizez" type="float">1</area_sizez>
+        </technique>
+      </extra>
+    </light>
+    <light id="Light-light" name="Light">
+      <technique_common>
+        <point>
+          <color sid="color">1000 1000 1000</color>
+          <constant_attenuation>1</constant_attenuation>
+          <linear_attenuation>0</linear_attenuation>
+          <quadratic_attenuation>0</quadratic_attenuation>
+        </point>
+      </technique_common>
+      <extra>
+        <technique profile="blender">
+          <type sid="type" type="int">0</type>
+          <flag sid="flag" type="int">0</flag>
+          <mode sid="mode" type="int">2097153</mode>
+          <red sid="red" type="float">1</red>
+          <green sid="green" type="float">1</green>
+          <blue sid="blue" type="float">1</blue>
+          <energy sid="blender_energy" type="float">1000</energy>
+          <spotsize sid="spotsize" type="float">75</spotsize>
+          <spotblend sid="spotblend" type="float">0.15</spotblend>
+          <clipsta sid="clipsta" type="float">0.04999995</clipsta>
+          <clipend sid="clipend" type="float">40</clipend>
+          <radius sid="radius" type="float">0.1</radius>
+          <area_shape sid="area_shape" type="int">1</area_shape>
+          <area_size sid="area_size" type="float">0.1</area_size>
+          <area_sizey sid="area_sizey" type="float">0.1</area_sizey>
+          <area_sizez sid="area_sizez" type="float">1</area_sizez>
+        </technique>
+      </extra>
+    </light>
+  </library_lights>
+  <library_effects>
+    <effect id="Material_001-effect">
+      <profile_COMMON>
+        <technique sid="common">
+          <lambert>
+            <emission>
+              <color sid="emission">0 0 0 1</color>
+            </emission>
+            <diffuse>
+              <color sid="diffuse">0.1584888 0.3396751 0.8005451 1</color>
+            </diffuse>
+            <index_of_refraction>
+              <float sid="ior">1.5</float>
+            </index_of_refraction>
+          </lambert>
+        </technique>
+      </profile_COMMON>
+    </effect>
+  </library_effects>
+  <library_images/>
+  <library_materials>
+    <material id="Material_001-material" name="Material.001">
+      <instance_effect url="#Material_001-effect"/>
+    </material>
+  </library_materials>
+  <library_geometries>
+    <geometry id="laser-mesh" name="laser">
+      <mesh>
+        <source id="laser-mesh-positions">
+          <float_array id="laser-mesh-positions-array" count="96">-0.09499996 0.004999995 0.03999996 -0.09499996 0.004999995 -0.03999996 -0.09499996 0.305 0.03999996 -0.09499996 0.305 -0.03999996 -0.175 0.004999995 -0.03999996 -0.175 0.305 -0.03999996 -0.175 0.004999995 0.03999996 -0.175 0.305 0.03999996 -0.225 0.004999995 -0.155 -0.225 0.004999995 0.155 0.225 0.004999995 0.155 0.225 0.004999995 -0.155 0.225 0 -0.155 0.225 0 0.155 -0.225 0 -0.155 -0.225 0 0.155 -0.225 0.455 -0.07249999 -0.175 0.455 -0.03999996 -0.09499996 0.455 -0.03999996 -0.175 0.455 0.03999996 -0.225 0.455 0.07249999 -0.09499996 0.455 0.03999996 0.175 0.455 0.07249999 0.175 0.455 -0.07249999 0.175 0.305 0.07249999 -0.225 0.305 0.07249999 0.175 0.305 -0.07249999 -0.225 0.305 -0.07249999 -0.175 0.605 -0.03999996 -0.175 0.605 0.03999996 -0.09499996 0.605 -0.03999996 -0.09499996 0.605 0.03999996</float_array>
+          <technique_common>
+            <accessor source="#laser-mesh-positions-array" count="32" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="laser-mesh-normals">
+          <float_array id="laser-mesh-normals-array" count="42">1 0 0 0 0 -1 -1 0 0 0 0 1 0 1 0 0 1 0 0 -1 0 0 1 2.8656e-6 0 1 -2.29248e-6 0 1 2.29248e-6 0 -1 1.14624e-6 0 -1 -5.73121e-7 0 -1 -2.8656e-6 0 -1 2.8656e-6</float_array>
+          <technique_common>
+            <accessor source="#laser-mesh-normals-array" count="14" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="laser-mesh-vertices">
+          <input semantic="POSITION" source="#laser-mesh-positions"/>
+        </vertices>
+        <triangles material="Material_001-material" count="60">
+          <input semantic="VERTEX" source="#laser-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#laser-mesh-normals" offset="1"/>
+          <p>0 0 1 0 2 0 2 0 1 0 3 0 1 1 4 1 3 1 3 1 4 1 5 1 4 2 6 2 5 2 5 2 6 2 7 2 6 3 0 3 7 3 7 3 0 3 2 3 4 4 8 4 6 4 6 4 8 4 9 4 6 4 9 4 0 4 0 5 9 5 10 5 0 4 10 4 1 4 1 4 10 4 11 4 1 4 11 4 4 4 4 4 11 4 8 4 11 0 10 0 12 0 12 0 10 0 13 0 8 1 11 1 14 1 14 1 11 1 12 1 9 2 8 2 15 2 15 2 8 2 14 2 10 3 9 3 13 3 13 3 9 3 15 3 15 6 14 6 13 6 13 6 14 6 12 6 16 7 17 7 18 7 17 4 16 4 19 4 19 4 16 4 20 4 19 4 20 4 21 4 21 8 20 8 22 8 21 4 22 4 18 4 18 4 22 4 23 4 18 9 23 9 16 9 2 10 24 10 25 10 24 6 2 6 26 6 26 6 2 6 3 6 26 11 3 11 27 11 27 12 3 12 5 12 27 6 5 6 25 6 25 6 5 6 7 6 25 13 7 13 2 13 23 0 22 0 26 0 26 0 22 0 24 0 16 1 23 1 27 1 27 1 23 1 26 1 20 2 16 2 25 2 25 2 16 2 27 2 22 3 20 3 24 3 24 3 20 3 25 3 28 4 29 4 30 4 30 4 29 4 31 4 21 3 31 3 19 3 19 3 31 3 29 3 19 2 29 2 17 2 17 2 29 2 28 2 17 1 28 1 18 1 18 1 28 1 30 1 18 0 30 0 21 0 21 0 30 0 31 0</p>
+        </triangles>
+      </mesh>
+    </geometry>
+  </library_geometries>
+  <library_visual_scenes>
+    <visual_scene id="Scene" name="Scene">
+      <node id="Light_001" name="Light.001" type="NODE">
+        <matrix sid="transform">-0.2908646 -0.7711008 0.5663933 4.076245 0.9551712 -0.1998833 0.2183913 1.005454 -0.05518912 0.6045247 0.7946723 5.903862 0 0 0 1</matrix>
+        <instance_light url="#Light_001-light"/>
+      </node>
+      <node id="Camera_001" name="Camera.001" type="NODE">
+        <matrix sid="transform">0.6859207 -0.3240135 0.6515582 7.358891 0.7276763 0.3054209 -0.6141704 -6.925791 -4.01133e-9 0.8953956 0.4452714 4.958309 0 0 0 1</matrix>
+        <instance_camera url="#Camera_001-camera"/>
+      </node>
+      <node id="laser" name="laser" type="NODE">
+        <matrix sid="transform">1 0 0 0 0 7.54979e-8 -1 0 0 1 7.54979e-8 0 0 0 0 1</matrix>
+        <instance_geometry url="#laser-mesh" name="laser">
+          <bind_material>
+            <technique_common>
+              <instance_material symbol="Material_001-material" target="#Material_001-material"/>
+            </technique_common>
+          </bind_material>
+        </instance_geometry>
+      </node>
+      <node id="Camera" name="Camera" type="NODE">
+        <matrix sid="transform">0.6859207 -0.3240135 0.6515582 7.358891 0.7276763 0.3054208 -0.6141704 -6.925791 0 0.8953956 0.4452714 4.958309 0 0 0 1</matrix>
+        <instance_camera url="#Camera-camera"/>
+      </node>
+      <node id="Light" name="Light" type="NODE">
+        <matrix sid="transform">-0.2908646 -0.7711008 0.5663932 4.076245 0.9551712 -0.1998834 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1</matrix>
+        <instance_light url="#Light-light"/>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#Scene"/>
+  </scene>
+</COLLADA>
--- a/rbs_mill_assist/assets/laser/meshes/laser.mtl
+++ b/rbs_mill_assist/assets/laser/meshes/laser.mtl
@ -0,0 +1,2 @@
+# Blender 4.3.2 MTL File: 'None'
+# www.blender.org
--- a/rbs_mill_assist/assets/laser/meshes/laser.obj
+++ b/rbs_mill_assist/assets/laser/meshes/laser.obj
@ -0,0 +1,103 @@
+# Blender 4.3.2
+# www.blender.org
+mtllib laser.mtl
+o laser
+v -0.095000 0.005000 0.040000
+v -0.095000 0.005000 -0.040000
+v -0.095000 0.305000 0.040000
+v -0.095000 0.305000 -0.040000
+v -0.175000 0.005000 -0.040000
+v -0.175000 0.305000 -0.040000
+v -0.175000 0.005000 0.040000
+v -0.175000 0.305000 0.040000
+v -0.225000 0.005000 -0.155000
+v -0.225000 0.005000 0.155000
+v 0.225000 0.005000 0.155000
+v 0.225000 0.005000 -0.155000
+v 0.225000 0.000000 -0.155000
+v 0.225000 -0.000000 0.155000
+v -0.225000 0.000000 -0.155000
+v -0.225000 -0.000000 0.155000
+v -0.225000 0.455000 -0.072500
+v -0.175000 0.455000 -0.040000
+v -0.095000 0.455000 -0.040000
+v -0.175000 0.455000 0.040000
+v -0.225000 0.455000 0.072500
+v -0.095000 0.455000 0.040000
+v 0.175000 0.455000 0.072500
+v 0.175000 0.455000 -0.072500
+v 0.175000 0.305000 0.072500
+v -0.225000 0.305000 0.072500
+v 0.175000 0.305000 -0.072500
+v -0.225000 0.305000 -0.072500
+v -0.175000 0.605000 -0.040000
+v -0.175000 0.605000 0.040000
+v -0.095000 0.605000 -0.040000
+v -0.095000 0.605000 0.040000
+vn 1.0000 -0.0000 -0.0000
+vn -0.0000 -0.0000 -1.0000
+vn -1.0000 -0.0000 -0.0000
+vn -0.0000 -0.0000 1.0000
+vn -0.0000 1.0000 -0.0000
+vn -0.0000 -1.0000 -0.0000
+s 0
+f 1//1 2//1 3//1
+f 3//1 2//1 4//1
+f 2//2 5//2 4//2
+f 4//2 5//2 6//2
+f 5//3 7//3 6//3
+f 6//3 7//3 8//3
+f 7//4 1//4 8//4
+f 8//4 1//4 3//4
+f 5//5 9//5 7//5
+f 7//5 9//5 10//5
+f 7//5 10//5 1//5
+f 1//5 10//5 11//5
+f 1//5 11//5 2//5
+f 2//5 11//5 12//5
+f 2//5 12//5 5//5
+f 5//5 12//5 9//5
+f 12//1 11//1 13//1
+f 13//1 11//1 14//1
+f 9//2 12//2 15//2
+f 15//2 12//2 13//2
+f 10//3 9//3 16//3
+f 16//3 9//3 15//3
+f 11//4 10//4 14//4
+f 14//4 10//4 16//4
+f 16//6 15//6 14//6
+f 14//6 15//6 13//6
+f 17//5 18//5 19//5
+f 18//5 17//5 20//5
+f 20//5 17//5 21//5
+f 20//5 21//5 22//5
+f 22//5 21//5 23//5
+f 22//5 23//5 19//5
+f 19//5 23//5 24//5
+f 19//5 24//5 17//5
+f 3//6 25//6 26//6
+f 25//6 3//6 27//6
+f 27//6 3//6 4//6
+f 27//6 4//6 28//6
+f 28//6 4//6 6//6
+f 28//6 6//6 26//6
+f 26//6 6//6 8//6
+f 26//6 8//6 3//6
+f 24//1 23//1 27//1
+f 27//1 23//1 25//1
+f 17//2 24//2 28//2
+f 28//2 24//2 27//2
+f 21//3 17//3 26//3
+f 26//3 17//3 28//3
+f 23//4 21//4 25//4
+f 25//4 21//4 26//4
+f 29//5 30//5 31//5
+f 31//5 30//5 32//5
+f 22//4 32//4 20//4
+f 20//4 32//4 30//4
+f 20//3 30//3 18//3
+f 18//3 30//3 29//3
+f 18//2 29//2 19//2
+f 19//2 29//2 31//2
+f 19//1 31//1 22//1
+f 22//1 31//1 32//1
--- a/rbs_mill_assist/assets/laser/meshes/laser.stl
+++ b/rbs_mill_assist/assets/laser/meshes/laser.stl
--- a/rbs_mill_assist/assets/laser/model.sdf
+++ b/rbs_mill_assist/assets/laser/model.sdf
@ -7,32 +7,32 @@
      <visual name="laser_visual">
        <geometry>
          <mesh>
-            <uri>model://laser/meshes/laser.stl</uri>
+            <uri>model://laser/meshes/laser.obj</uri>
          </mesh>
        </geometry>
-        <!-- <material> -->
-        <!--   <diffuse>1 1 1 1</diffuse> -->
-        <!--   <ambient>1 1 1 1</ambient> -->
-        <!--   <specular>0.5 0.5 0.5 1</specular> -->
-        <!--   <emissive>0 0 0 1</emissive> -->
-        <!--   <texture> -->
-        <!--     <diffuse_map>model://laser/textures/shildik_sh.png</diffuse_map> -->
-        <!--   </texture> -->
-        <!--   <pbr> -->
-        <!--     <metal> -->
-        <!--       <albedo_map>model://laser/textures/shildik_sh_d.png</albedo_map> -->
-        <!--       <normal_map>model://laser/textures/shildik_sh_n.png</normal_map> -->
-        <!--       <roughness_map>model://laser/textures/shildik_sh_r.png</roughness_map> -->
-        <!--       <metalness_map>model://laser/textures/shildik_sh_m.png</metalness_map> -->
-        <!--       <ambient_occlusion_map>model://laser/textures/shildik_sh_o.png</ambient_occlusion_map> -->
-        <!--     </metal> -->
-        <!--   </pbr> -->
-        <!-- </material> -->
+        <material>
+          <diffuse>1 1 1 1</diffuse>
+          <ambient>1 1 1 1</ambient>
+          <specular>0.5 0.5 0.5 1</specular>
+          <emissive>0 0 0 1</emissive>
+          <!-- <texture> -->
+          <!--   <diffuse_map>model://laser/textures/shildik_sh.png</diffuse_map> -->
+          <!-- </texture> -->
+          <!-- <pbr> -->
+          <!--   <metal> -->
+          <!--     <albedo_map>model://laser/textures/shildik_sh_d.png</albedo_map> -->
+          <!--     <normal_map>model://laser/textures/shildik_sh_n.png</normal_map> -->
+          <!--     <roughness_map>model://laser/textures/shildik_sh_r.png</roughness_map> -->
+          <!--     <metalness_map>model://laser/textures/shildik_sh_m.png</metalness_map> -->
+          <!--     <ambient_occlusion_map>model://laser/textures/shildik_sh_o.png</ambient_occlusion_map> -->
+          <!--   </metal> -->
+          <!-- </pbr> -->
+        </material>
      </visual>
      <collision name="laser_collision">
        <geometry>
          <mesh>
-            <uri>model://laser/meshes/laser.stl</uri>
+            <uri>model://laser/meshes/laser.obj</uri>
          </mesh>
        </geometry>
        <surface>
--- a/rbs_mill_assist/assets/shildik/meshes/shildik_sh.dae
+++ b/rbs_mill_assist/assets/shildik/meshes/shildik_sh.dae
@ -3,13 +3,66 @@
  <asset>
    <contributor>
      <author>Blender User</author>
-      <authoring_tool>Blender 4.1.1 commit date:2024-04-15, commit time:15:11, hash:e1743a0317bc</authoring_tool>
+      <authoring_tool>Blender 4.3.2 commit date:2024-12-16, commit time:21:10, hash:32f5fdce0a0a</authoring_tool>
    </contributor>
-    <created>2025-02-12T10:55:51</created>
-    <modified>2025-02-12T10:55:51</modified>
+    <created>2025-03-03T12:34:37</created>
+    <modified>2025-03-03T12:34:37</modified>
    <unit name="meter" meter="1"/>
    <up_axis>Z_UP</up_axis>
  </asset>
+  <library_cameras>
+    <camera id="Camera-camera" name="Camera">
+      <optics>
+        <technique_common>
+          <perspective>
+            <xfov sid="xfov">39.59775</xfov>
+            <aspect_ratio>1.777778</aspect_ratio>
+            <znear sid="znear">0.1</znear>
+            <zfar sid="zfar">100</zfar>
+          </perspective>
+        </technique_common>
+      </optics>
+      <extra>
+        <technique profile="blender">
+          <shiftx sid="shiftx" type="float">0</shiftx>
+          <shifty sid="shifty" type="float">0</shifty>
+          <dof_distance sid="dof_distance" type="float">10</dof_distance>
+        </technique>
+      </extra>
+    </camera>
+  </library_cameras>
+  <library_lights>
+    <light id="Light-light" name="Light">
+      <technique_common>
+        <point>
+          <color sid="color">1000 1000 1000</color>
+          <constant_attenuation>1</constant_attenuation>
+          <linear_attenuation>0</linear_attenuation>
+          <quadratic_attenuation>0</quadratic_attenuation>
+        </point>
+      </technique_common>
+      <extra>
+        <technique profile="blender">
+          <type sid="type" type="int">0</type>
+          <flag sid="flag" type="int">0</flag>
+          <mode sid="mode" type="int">2097153</mode>
+          <red sid="red" type="float">1</red>
+          <green sid="green" type="float">1</green>
+          <blue sid="blue" type="float">1</blue>
+          <energy sid="blender_energy" type="float">1000</energy>
+          <spotsize sid="spotsize" type="float">75</spotsize>
+          <spotblend sid="spotblend" type="float">0.15</spotblend>
+          <clipsta sid="clipsta" type="float">0.04999995</clipsta>
+          <clipend sid="clipend" type="float">40</clipend>
+          <radius sid="radius" type="float">0.1</radius>
+          <area_shape sid="area_shape" type="int">1</area_shape>
+          <area_size sid="area_size" type="float">0.1</area_size>
+          <area_sizey sid="area_sizey" type="float">0.1</area_sizey>
+          <area_sizez sid="area_sizez" type="float">1</area_sizez>
+        </technique>
+      </extra>
+    </light>
+  </library_lights>
  <library_effects>
    <effect id="nasosnaya_ustanovka_svg-effect">
      <profile_COMMON>
@ -39,7 +92,7 @@
    <geometry id="shildik-mesh" name="shildik">
      <mesh>
        <source id="shildik-mesh-positions">
-          <float_array id="shildik-mesh-positions-array" count="54">-0.02999997 -0.01999998 0 -0.02999997 -0.01999998 5e-4 -0.02999997 0.01999998 0 -0.02999997 0.01999998 5e-4 0.02999997 -0.01999998 0 0.02999997 -0.01999998 5e-4 0.02999997 0.01999998 0 0.02999997 0.01999998 5e-4 -0.02999997 0 0 -0.02999997 0 5e-4 0 0.01999998 0 0 0.01999998 5e-4 0.02999997 0 0 0.02999997 0 5e-4 0 -0.01999998 0 0 -0.01999998 5e-4 0 0 5e-4 0 0 0</float_array>
+          <float_array id="shildik-mesh-positions-array" count="54">-0.02999997 -0.01999998 -2.5e-4 -0.02999997 -0.01999998 2.5e-4 -0.02999997 0.01999998 -2.5e-4 -0.02999997 0.01999998 2.5e-4 0.02999997 -0.01999998 -2.5e-4 0.02999997 -0.01999998 2.5e-4 0.02999997 0.01999998 -2.5e-4 0.02999997 0.01999998 2.5e-4 -0.02999997 0 -2.5e-4 -0.02999997 0 2.5e-4 0 0.01999998 -2.5e-4 0 0.01999998 2.5e-4 0.02999997 0 -2.5e-4 0.02999997 0 2.5e-4 0 -0.01999998 -2.5e-4 0 -0.01999998 2.5e-4 0 0 2.5e-4 0 0 -2.5e-4</float_array>
          <technique_common>
            <accessor source="#shildik-mesh-positions-array" count="18" stride="3">
              <param name="X" type="float"/>
@ -87,12 +140,20 @@
          <bind_material>
            <technique_common>
              <instance_material symbol="nasosnaya_ustanovka_svg-material" target="#nasosnaya_ustanovka_svg-material">
-                <bind_vertex_input semantic="UVMap" input_semantic="TEXCOORD" input_set="0"/>
+                <bind_vertex_input semantic="shildik-mesh-map-0" input_semantic="TEXCOORD" input_set="0"/>
              </instance_material>
            </technique_common>
          </bind_material>
        </instance_geometry>
      </node>
+      <node id="Camera" name="Camera" type="NODE">
+        <matrix sid="transform">0.6859207 -0.3240135 0.6515582 7.358891 0.7276763 0.3054208 -0.6141704 -6.925791 0 0.8953956 0.4452714 4.958309 0 0 0 1</matrix>
+        <instance_camera url="#Camera-camera"/>
+      </node>
+      <node id="Light" name="Light" type="NODE">
+        <matrix sid="transform">-0.2908646 -0.7711008 0.5663932 4.076245 0.9551712 -0.1998834 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1</matrix>
+        <instance_light url="#Light-light"/>
+      </node>
    </visual_scene>
  </library_visual_scenes>
  <scene>
--- a/rbs_mill_assist/assets/shildik/model.sdf
+++ b/rbs_mill_assist/assets/shildik/model.sdf
@ -31,7 +31,7 @@
      <collision name="shildik_collision">
        <geometry>
          <box>
-            <size>0.06 0.04 0.001</size>
+            <size>0.06 0.04 0.0005</size>
          </box>
        </geometry>
        <surface>
--- a/rbs_mill_assist/bt/CMakeLists.txt
+++ b/rbs_mill_assist/bt/CMakeLists.txt
@ -2,8 +2,9 @@
 find_package(behaviortree_ros2 REQUIRED)
 find_package(behaviortree_cpp REQUIRED)
 find_package(rbs_skill_interfaces REQUIRED)
+find_package(rbs_utils_interfaces REQUIRED)
 find_package(geometry_msgs REQUIRED)
-# find_package(std_srvs REQUIRED)
+find_package(std_srvs REQUIRED)

 # Behaviortree interfaces
 set(dependencies
@ -11,6 +12,7 @@ set(dependencies
    rbs_skill_interfaces
    geometry_msgs
    behaviortree_ros2
+    rbs_utils_interfaces
    std_srvs
 )

@ -20,6 +22,15 @@ list(APPEND plugin_libs vacuum_gripper_toggle)
 add_library(get_grasp_place_pose SHARED plugins/get_grasp_place_pose.cpp)
 list(APPEND plugin_libs get_grasp_place_pose)

+add_library(get_named_pose SHARED plugins/get_named_pose.cpp)
+list(APPEND plugin_libs get_named_pose)
+
+add_library(is_in_pose SHARED plugins/is_in_pose.cpp)
+list(APPEND plugin_libs is_in_pose)
+
+add_library(twist_command_with_condition SHARED plugins/twist_cmd.cpp)
+list(APPEND plugin_libs twist_command_with_condition)
+
 foreach(bt_plugin ${plugin_libs})
    ament_target_dependencies(${bt_plugin} ${dependencies})
    target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
--- a/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp
+++ b/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp
@ -34,11 +34,12 @@ public:
  }

  bool setRequest(Request::SharedPtr &request) override {
-    RCLCPP_INFO(this->logger(), "Starting send request for: [%s]",
-                this->service_name_.c_str());
+    RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]",
+                name().c_str(), this->service_name_.c_str());
    if (!getInput("action_name", request->action_name)) {
      RCLCPP_ERROR(this->node_.lock()->get_logger(),
-                   "Failed to get object_name from input port");
+                   "[%s] Failed to get object_name from input port",
+                   name().c_str());
      return false;
    }
    return true;
@ -47,25 +48,25 @@ public:
  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
    if (!response->ok) {
      RCLCPP_ERROR(this->node_.lock()->get_logger(),
-                   "Response indicates failure.");
+                   "[%s] Response indicates failure.", name().c_str());
      return NodeStatus::FAILURE;
    }

    RCLCPP_INFO(this->node_.lock()->get_logger(),
-                "Response received successfully.");
+                "[%s] Response received successfully.", name().c_str());

    auto logPose = [this](const std::string &pose_name,
                          const geometry_msgs::msg::Pose &pose) {
      RCLCPP_INFO(this->node_.lock()->get_logger(),
-                  "%s:\n"
+                  "[%s] %s:\n"
                  "  Position: x = %.4f, y = %.4f, z = %.4f\n"
                  "  Orientation: x = %.4f, y = %.4f, z = %.4f, w = %.4f",
-                  pose_name.c_str(), pose.position.x, pose.position.y,
-                  pose.position.z, pose.orientation.x, pose.orientation.y,
-                  pose.orientation.z, pose.orientation.w);
+                  name().c_str(), pose_name.c_str(), pose.position.x,
+                  pose.position.y, pose.position.z, pose.orientation.x,
+                  pose.orientation.y, pose.orientation.z, pose.orientation.w);
    };
    if (!response->grasp.empty()) {
-      RCLCPP_INFO(this->logger(), "Got Pick Response");
+      RCLCPP_INFO(this->logger(), "[%s] Got Pick Response", name().c_str());
      std::vector<std::string> poses = {"Pregrasp Pose", "Grasp Pose",
                                        "Postgrasp Pose"};

@ -87,7 +88,7 @@ public:
      return NodeStatus::SUCCESS;
    }
    if (!response->place.empty()) {
-      RCLCPP_INFO(this->logger(), "Got Place Response");
+      RCLCPP_INFO(this->logger(), "[%s] Got Place Response", name().c_str());
      std::vector<std::string> poses = {"Preplace Pose", "Place Pose",
                                        "Postplace Pose"};

@ -109,7 +110,7 @@ public:
      return NodeStatus::SUCCESS;
    }

-    RCLCPP_FATAL(this->logger(), "Could not response grasp pose");
+    RCLCPP_FATAL(this->logger(), "[%s] Could not response grasp pose", name().c_str());
    return NodeStatus::FAILURE;
  }
  // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
--- a/rbs_mill_assist/bt/plugins/get_named_pose.cpp
+++ b/rbs_mill_assist/bt/plugins/get_named_pose.cpp
@ -0,0 +1,65 @@
+#include "behaviortree_ros2/bt_service_node.hpp"
+
+#include "behaviortree_ros2/plugins.hpp"
+#include <behaviortree_cpp/basic_types.h>
+// #include <geometry_msgs/msg/detail/point__struct.hpp>
+#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
+#include "rbs_utils_interfaces/srv/get_named_pose.hpp"
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
+#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
+#include <geometry_msgs/msg/detail/quaternion__struct.hpp>
+#include <memory>
+#include <rclcpp/logging.hpp>
+#include <string>
+
+using GetNamedPoseService = rbs_utils_interfaces::srv::GetNamedPose;
+using namespace BT;
+
+class GetNamedPose : public RosServiceNode<GetNamedPoseService> {
+public:
+  GetNamedPose(const std::string &name, const NodeConfig &conf,
+               const RosNodeParams &params)
+      : RosServiceNode<GetNamedPoseService>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting GetGraspPose");
+  }
+
+  static PortsList providedPorts() {
+    return providedBasicPorts(
+        {InputPort<std::string>("pose_name"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("output_pose")});
+  }
+
+  bool setRequest(Request::SharedPtr &request) override {
+    RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]",
+                name().c_str(), this->service_name_.c_str());
+    if (!getInput("pose_name", request->pose_name)) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "[%s] Failed to get pose_name from input port",
+                   name().c_str());
+      return false;
+    }
+    return true;
+  }
+
+  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
+    if (!response->ok) {
+      RCLCPP_ERROR(this->logger(), "[%s] Response indicates failure.",
+                   name().c_str());
+      return NodeStatus::FAILURE;
+    }
+
+    RCLCPP_INFO(this->logger(),
+                "[%s] Response received successfully with pose name [%s]",
+                name().c_str(), response->named_pose.name.c_str());
+
+    auto pose = std::make_shared<geometry_msgs::msg::Pose>();
+    *pose = response->named_pose.pose;
+    setOutput("output_pose", pose);
+
+    return NodeStatus::SUCCESS;
+  }
+  // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
+};
+
+CreateRosNodePlugin(GetNamedPose, "GetNamedPose");
--- a/rbs_mill_assist/bt/plugins/is_in_pose.cpp
+++ b/rbs_mill_assist/bt/plugins/is_in_pose.cpp
@ -0,0 +1,48 @@
+#include "behaviortree_ros2/bt_topic_sub_node.hpp"
+
+#include "behaviortree_ros2/plugins.hpp"
+#include <behaviortree_cpp/basic_types.h>
+// #include <geometry_msgs/msg/detail/point__struct.hpp>
+#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
+#include "rbs_utils_interfaces/srv/get_named_pose.hpp"
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
+#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
+#include <geometry_msgs/msg/detail/quaternion__struct.hpp>
+#include <memory>
+#include <rclcpp/logging.hpp>
+#include <string>
+
+using MSG = geometry_msgs::msg::Pose;
+using namespace BT;
+
+class IsInPose : public RosTopicSubNode<MSG> {
+public:
+  IsInPose(const std::string &name, const NodeConfig &conf,
+           const RosNodeParams &params)
+      : RosTopicSubNode<MSG>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting IsInPose");
+  }
+  //
+  // static PortsList providedPorts() {
+  //   return {};
+  // }
+
+  NodeStatus onTick(const std::shared_ptr<MSG> &t_last_msg) override {
+    if (t_last_msg) {
+      RCLCPP_INFO(this->logger(), "[%s] Got message Pose", name().c_str());
+      m_last_msg = t_last_msg;
+      return NodeStatus::SUCCESS;
+    }
+    RCLCPP_INFO(this->logger(), "[%s] NOT message received", name().c_str());
+    return NodeStatus::SUCCESS;
+  }
+  bool latchLastMessage() const override {
+    return true;
+  }
+
+private:
+  MSG::SharedPtr m_last_msg;
+};
+
+CreateRosNodePlugin(IsInPose, "IsInPose");
--- a/rbs_mill_assist/bt/plugins/twist_cmd.cpp
+++ b/rbs_mill_assist/bt/plugins/twist_cmd.cpp
@ -0,0 +1,85 @@
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "rbs_skill_interfaces/action/twist_control_with_condition.hpp"
+#include <behaviortree_cpp/tree_node.h>
+#include <behaviortree_ros2/plugins.hpp>
+#include <behaviortree_ros2/ros_node_params.hpp>
+#include <rclcpp/logging.hpp>
+
+using namespace BT;
+using TwistCmdWithCondAction =
+    rbs_skill_interfaces::action::TwistControlWithCondition;
+
+class TwistCmdWithCondClient : public RosActionNode<TwistCmdWithCondAction> {
+public:
+  TwistCmdWithCondClient(const std::string &name, const NodeConfig &conf,
+                         const RosNodeParams &params)
+      : RosActionNode<TwistCmdWithCondAction>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting MoveToPose");
+  }
+
+  static BT::PortsList providedPorts() {
+    return providedBasicPorts({BT::InputPort<std::string>("robot_name"),
+                               BT::InputPort<std::vector<double>>("twist"),
+                               // BT::InputPort<std::string>("condition_topic")
+    });
+  }
+
+  bool setGoal(RosActionNode::Goal &goal) override {
+    RCLCPP_INFO(this->logger(), "[%s] Starting send goal [%s]", name().c_str(),
+                this->action_name_.c_str());
+
+    if (!getInput("robot_name", goal.robot_name)) {
+      RCLCPP_FATAL(this->logger(), "[%s] Could not get robot name",
+                   name().c_str());
+      return false;
+    }
+
+    if (!getInput("twist", twist_cmd_vec)) {
+      RCLCPP_FATAL(this->logger(), "[%s] Could not get twist command",
+                   name().c_str());
+      return false;
+    }
+    if (!twist_cmd_vec.empty() && twist_cmd_vec.size() == 6) {
+      goal.twist_cmd.linear.x = twist_cmd_vec[0];
+      goal.twist_cmd.linear.y = twist_cmd_vec[1];
+      goal.twist_cmd.linear.z = twist_cmd_vec[2];
+      goal.twist_cmd.angular.x = twist_cmd_vec[3];
+      goal.twist_cmd.angular.y = twist_cmd_vec[4];
+      goal.twist_cmd.angular.z = twist_cmd_vec[5];
+    } else {
+      RCLCPP_FATAL(this->logger(),
+                   "[%s] Twist command is empty or should be 1x6 [x;y;z;r;p;y]",
+                   name().c_str());
+      return false;
+    }
+
+    // if (!getInput("condition_topic", goal.condition_topic)) {
+    //   RCLCPP_FATAL(this->logger(), "[%s] Could not get condition_topic",
+    //                name().c_str());
+    //   return false;
+    // } else {
+    //   RCLCPP_INFO(this->logger(), "[%s] Using condition_topic [%s]",
+    //               name().c_str(), goal.condition_topic.c_str());
+    // }
+
+    return true;
+  }
+
+  NodeStatus onResultReceived(const WrappedResult &wr) override {
+
+    RCLCPP_INFO(this->logger(), "[%s] Starting get response %s with status %b",
+                name().c_str(), this->action_name_.c_str(), wr.result->success);
+    if (!wr.result->success) {
+      return NodeStatus::FAILURE;
+    }
+    return NodeStatus::SUCCESS;
+  }
+
+private:
+  // std::shared_ptr<geometry_msgs::msg::Pose> m_target_pose;
+  std::vector<double> twist_cmd_vec;
+};
+
+CreateRosNodePlugin(TwistCmdWithCondClient, "TwistCmdWithCond");
--- a/rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp
+++ b/rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp
@ -24,10 +24,10 @@ public:
  }

  bool setRequest(Request::SharedPtr &request) override {
-    RCLCPP_INFO(this->logger(), "Starting send request");
+    RCLCPP_INFO(this->logger(), "[%s] Starting send request", name().c_str());
    if (!getInput("enable", request->data)) {
-      RCLCPP_ERROR(this->node_.lock()->get_logger(),
-                   "Failed to get sending data from input port");
+      RCLCPP_ERROR(this->logger(),
+                   "[%s] Failed to get sending data from input port", name().c_str());
      return false;
    }
    return true;
@ -35,7 +35,7 @@ public:

  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
    if (!response->success) {
-      RCLCPP_ERROR(this->logger(), "Response indicates failure.");
+      RCLCPP_ERROR(this->logger(), "[%s] Response indicates failure.", name().c_str());
      return NodeStatus::FAILURE;
    }

--- a/rbs_mill_assist/bt/xmls/Grasp.xml
+++ b/rbs_mill_assist/bt/xmls/Grasp.xml
@ -3,19 +3,35 @@
  <BehaviorTree ID="Grasp">
    <Sequence>

-      <Script code="action:='/mtp_jtc'"/>
-      <Script code="get_workspace:='/get_workspace'"/>
+      <Action ID="MoveToPose" pose="{pregrasp}" robot_name="{robot_name}" duration="4" action_name="/mtp_moveit" />
+      <Action ID="ToggleVacuumGrippper" enable="true" service_name="/gz_ros2_vacuum_gripper_plugin/toggle" />

-      <Action ID="GetGraspPlacePose" action_name="pick" pre_pose="{pregrasp}" pose="{grasp}" post_pose="{postgrasp}" service_name="/get_pick_place_poses"/>
+      <Action ID="TwistCmdWithCond" action_name="/twist_cmd" robot_name="arm" twist="0;0;-4.0;0;0;0"/>

-      <Action ID="MoveToPose" pose="{pregrasp}" robot_name="{robot_name}" duration="2" action_name="{action}"  />
-      <Action ID="ToggleVacuumGrippper" enable="true" service_name="/gz_ros2_vacuum_gripper_plugin/toggle"  />
-      <Action ID="MoveToPose" pose="{grasp}" robot_name="{robot_name}" duration="2" action_name="{action}"  />
-      <Action ID="MoveToPose" pose="{postgrasp}" robot_name="{robot_name}" duration="2" action_name="{action}"  />
+      <!-- <Action ID="MoveToPose" pose="{grasp}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" /> -->
+      <Action ID="MoveToPose" pose="{postgrasp}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" />

    </Sequence>
  </BehaviorTree>

  <TreeNodesModel>
+    <Action ID="MoveToPose" editable="true">
+      <input_port name="pose"/>
+      <input_port name="robot_name"/>
+      <input_port name="duration"/>
+      <input_port name="action_name"/>
+    </Action>
+    <Action ID="ToggleVacuumGrippper" editable="true">
+      <input_port name="enable"/>
+      <input_port name="service_name"/>
+      <input_port name="robot_name"/>
+    </Action>
+
+    <Action ID="TwistCmdWithCond" editable="true">
+      <input_port name="robot_name"/>
+      <input_port name="twist"/>
+      <!-- <input_port name="duration"/> -->
+      <input_port name="action_name"/>
+    </Action>
  </TreeNodesModel>
 </root>
--- a/rbs_mill_assist/bt/xmls/MainTree.xml
+++ b/rbs_mill_assist/bt/xmls/MainTree.xml
@ -1,9 +1,112 @@
 <?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4">
  <BehaviorTree ID="Main">
+
    <Sequence>
-      <SubTree ID="Grasp"/>
-      <SubTree ID="Place"/>
+      <!-- <Action ID="GetTask" name="" data="{task}" num_cycles="{num_cycles}"/> -->
+
+      <Fallback>
+        <Repeat num_cycles="1">
+          <Sequence>
+            <!-- Preparing -->
+
+            <Action ID="GetGraspPlacePose" action_name="to_graver.pick"
+              pre_pose="{pregrasp}"
+              pose="{grasp}"
+              post_pose="{postgrasp}"
+              service_name="/get_pick_place_poses" />
+
+            <Action ID="GetGraspPlacePose" action_name="to_graver.place"
+              pre_pose="{preplace}"
+              pose="{place}"
+              post_pose="{postplace}"
+              service_name="/get_pick_place_poses" />
+
+            <!-- Grasp shildik and place to the Graver -->
+            <SubTree ID="PickAndPlace"
+              robot_name="arm"
+              preplace="{preplace}"
+              place="{place}"
+              postplace="{postplace}"
+              pregrasp="{pregrasp}"
+              grasp="{grasp}"
+              postgrasp="{postgrasp}"
+            />
+
+
+            <!-- Waiting position -->
+            <Action ID="GetNamedPose" pose_name="waiting" output_pose="{named_pose}"
+              service_name="/get_named_pose" />
+            <Action ID="MoveToPose" pose="{named_pose}" robot_name="arm" duration="3"
+              action_name="/mtp_jtc" />
+
+            <!-- Send task and wait Graver -->
+
+            <!-- <Action ID="SendTask" /> -->
+
+            <!-- Pick shildik from graver-->
+
+            <Action ID="GetGraspPlacePose" action_name="from_graver.pick"
+              pre_pose="{pregrasp}"
+              pose="{grasp}"
+              post_pose="{postgrasp}"
+              service_name="/get_pick_place_poses" />
+
+            <Action ID="GetGraspPlacePose" action_name="from_graver.place"
+              pre_pose="{preplace}"
+              pose="{place}"
+              post_pose="{postplace}"
+              service_name="/get_pick_place_poses" />
+
+            <!-- Grasp shildik from Graver and move to the box-->
+            <SubTree ID="PickAndPlace"
+              robot_name="arm"
+              preplace="{preplace}"
+              place="{place}"
+              postplace="{postplace}"
+              pregrasp="{pregrasp}"
+              grasp="{grasp}"
+              postgrasp="{postgrasp}"
+            />
+
+            <!-- Ending -->
+          </Sequence>
+        </Repeat>
+
+        <Sequence>
+          <!-- <Action ID="NotifyOperator" /> -->
+          <AlwaysFailure />
+        </Sequence>
+      </Fallback>
    </Sequence>
+
  </BehaviorTree>
+
+  <TreeNodesModel>
+    <Action ID="GetGraspPlacePose" editable="true">
+      <input_port name="action_name"/>
+      <input_port name="pre_pose"/>
+      <input_port name="pose"/>
+      <input_port name="post_pose"/>
+      <input_port name="service_name"/>
+    </Action>
+
+    <Action ID="GetNamedPose" editable="true">
+      <input_port name="pose_name"/>
+      <input_port name="output_pose"/>
+      <input_port name="service_name"/>
+    </Action>
+
+    <Action ID="NotifyOperator" editable="true" />
+
+    <Action ID="GetTask" editable="true">
+      <input_port name="name"/> 
+      <input_port name="data"/>
+      <input_port name="num_cycles"/> 
+    </Action>
+
+    <Action ID="SendTask" editable="true" />
+  </TreeNodesModel>
 </root>
+
+
--- a/rbs_mill_assist/bt/xmls/PickAndPlace.xml
+++ b/rbs_mill_assist/bt/xmls/PickAndPlace.xml
@ -0,0 +1,9 @@
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="PickAndPlace">
+    <Sequence>
+      <SubTree ID="Grasp" pregrasp="{pregrasp}" grasp="{grasp}" postgrasp="{grasp}" robot_name="{robot_name}"/>
+      <SubTree ID="Place" preplace="{preplace}" place="{place}" postplace="{postplace}" robot_name="{robot_name}"/>
+    </Sequence>
+  </BehaviorTree>
+</root>
--- a/rbs_mill_assist/bt/xmls/Place.xml
+++ b/rbs_mill_assist/bt/xmls/Place.xml
@ -3,19 +3,25 @@
  <BehaviorTree ID="Place">
    <Sequence>

-      <Script code="action:='/mtp_jtc'"/>
-      <Script code="get_workspace:='/get_workspace'"/>
-
-      <Action ID="GetGraspPlacePose" action_name="place" pre_pose="{preplace}" pose="{place}" post_pose="{postplace}" service_name="/get_pick_place_poses"/>
-
-      <Action ID="MoveToPose" pose="{preplace}" robot_name="{robot_name}" duration="2" action_name="{action}"  />
-      <Action ID="MoveToPose" pose="{place}" robot_name="{robot_name}" duration="2" action_name="{action}"  />
-      <Action ID="ToggleVacuumGrippper" enable="false" service_name="/gz_ros2_vacuum_gripper_plugin/toggle"  />
-      <Action ID="MoveToPose" pose="{postplace}" robot_name="{robot_name}" duration="2" action_name="{action}"  />
+      <Action ID="MoveToPose" pose="{preplace}" robot_name="{robot_name}" duration="4" action_name="/mtp_moveit" />
+      <Action ID="MoveToPose" pose="{place}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" />
+      <Action ID="ToggleVacuumGrippper" enable="false" service_name="/gz_ros2_vacuum_gripper_plugin/toggle" />
+      <Action ID="MoveToPose" pose="{postplace}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" />

    </Sequence>
  </BehaviorTree>

  <TreeNodesModel>
+    <Action ID="MoveToPose" editable="true">
+      <input_port name="pose"/>
+      <input_port name="robot_name"/>
+      <input_port name="duration"/>
+      <input_port name="action_name"/>
+    </Action>
+    <Action ID="ToggleVacuumGrippper" editable="true">
+      <input_port name="enable"/>
+      <input_port name="service_name"/>
+      <input_port name="robot_name"/>
+    </Action>
  </TreeNodesModel>
 </root>
--- a/rbs_mill_assist/bt/xmls/btmill.btproj
+++ b/rbs_mill_assist/bt/xmls/btmill.btproj
@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4" project_name="Project">
+    <include path="Grasp.xml"/>
+    <include path="Place.xml"/>
+    <include path="MainTree.xml"/>
+    <include path="PickAndPlace.xml"/>
+    <!-- Description of Node Models (used by Groot) -->
+    <TreeNodesModel>
+    </TreeNodesModel>
+</root>
--- a/rbs_mill_assist/config/grasping.yaml
+++ b/rbs_mill_assist/config/grasping.yaml
@ -1,60 +1,124 @@
-pregrasp_pose:
-  position:
-    x: 0.10395
-    y: -0.28
-    z: 0.1
-  orientation:
-    x: 0.0
-    y: 1.0
-    z: 0.0
-    w: 0.0
-grasp_pose:
-  position:
-    x: 0.10395
-    y: -0.28
-    z: 0.004
-  orientation:
-    x: 0.0
-    y: 1.0
-    z: 0.0
-    w: 0.04
-postgrasp_pose:
-  position:
-    x: 0.10395
-    y: -0.28
-    z: 0.1
-  orientation:
-    x: 0.0
-    y: 1.0
-    z: 0.0
-    w: 0.0
-preplace_pose:
-  position:
-    x: 0.360
-    y: -0.06
-    z: 0.1
-  orientation:
-    x: -0.707
-    y: 0.707
-    z: 0.0
-    w: 0.0
-place_pose:
-  position:
-    x: 0.360
-    y: -0.06
-    z: 0.05
-  orientation:
-    x: -0.707
-    y: 0.707
-    z: 0.0
-    w: 0.0
-postplace_pose:
-  position:
-    x: 0.360
-    y: -0.06
-    z: 0.1
-  orientation:
-    x: -0.707
-    y: 0.707
-    z: 0.0
-    w: 0.0
+to_graver:
+  pregrasp_pose:
+    position:
+      x: -0.3
+      y: -0.20
+      z: 0.2
+    orientation:
+      x: 0.707
+      y: -0.707
+      z: 0.0
+      w: 0.0
+  grasp_pose:
+    position:
+      x: -0.3
+      y: -0.20
+      z: 0.001
+    orientation:
+      x: 0.707
+      y: -0.707
+      z: 0.0
+      w: 0.0
+  postgrasp_pose:
+    position:
+      x: -0.3
+      y: -0.20
+      z: 0.2
+    orientation:
+      x: 0.707
+      y: -0.707
+      z: 0.0
+      w: 0.0
+
+  preplace_pose:
+    position:
+      x: 0.40
+      y: 0.0
+      z: 0.1
+    orientation:
+      x: 0.707
+      y: 0.707
+      z: 0.0
+      w: 0.0
+  place_pose:
+    position:
+      x: 0.40
+      y: 0.0
+      z: 0.015
+    orientation:
+      x: 0.707
+      y: 0.707
+      z: 0.0
+      w: 0.0
+  postplace_pose:
+    position:
+      x: 0.40
+      y: 0.0
+      z: 0.1
+    orientation:
+      x: 0.707
+      y: 0.707
+      z: 0.0
+      w: 0.0
+
+from_graver:
+  pregrasp_pose:
+    position:
+      x: 0.40
+      y: 0.0
+      z: 0.1
+    orientation:
+      x: 0.707
+      y: 0.707
+      z: 0.0
+      w: 0.0
+  grasp_pose:
+    position:
+      x: 0.40
+      y: 0.0
+      z: 0.015
+    orientation:
+      x: 0.707
+      y: 0.707
+      z: 0.0
+      w: 0.0
+  postgrasp_pose:
+    position:
+      x: 0.40
+      y: 0.0
+      z: 0.1
+    orientation:
+      x: 0.707
+      y: 0.707
+      z: 0.0
+      w: 0.0
+  preplace_pose:
+    position:
+      x: -0.3
+      y: 0.20
+      z: 0.2
+    orientation:
+      x: 0.707
+      y: -0.707
+      z: 0.0
+      w: 0.0
+  place_pose:
+    position:
+      x: -0.3
+      y: 0.20
+      z: 0.0
+    orientation:
+      x: 0.707
+      y: -0.707
+      z: 0.0
+      w: 0.0
+  postplace_pose:
+    position:
+      x: -0.3
+      y: 0.20
+      z: 0.2
+    orientation:
+      x: 0.707
+      y: -0.707
+      z: 0.0
+      w: 0.0
--- a/rbs_mill_assist/config/gz_bridge.yaml
+++ b/rbs_mill_assist/config/gz_bridge.yaml
@ -3,7 +3,23 @@
  gz_type_name: "gz.msgs.Clock"
  direction: GZ_TO_ROS

- topic_name: "rgbd_camera/image"
+- topic_name: "/rgbd_camera/image/image"
  ros_type_name: "sensor_msgs/msg/Image"
  gz_type_name: "gz.msgs.Image"
  direction: GZ_TO_ROS
+
+- topic_name: "/rgbd_camera/image/depth_image"
+  ros_type_name: "sensor_msgs/msg/Image"
+  gz_type_name: "gz.msgs.Image"
+  direction: GZ_TO_ROS
+
+- topic_name: "/rgbd_camera/image/camera_info"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "gz.msgs.CameraInfo"
+  direction: GZ_TO_ROS
+
+# TODO: add static TF publishing of the camera frame
+# - topic_name: "/rgbd_camera/image/points"
+#   ros_type_name: "sensor_msgs/msg/PointCloud2"
+#   gz_type_name: "gz.msgs.PointCloudPacked"
+#   direction: GZ_TO_ROS
--- a/rbs_mill_assist/config/key_poses.yaml
+++ b/rbs_mill_assist/config/key_poses.yaml
@ -0,0 +1,10 @@
+waiting:
+  position:
+    x: 0.23034882653403538
+    y: -0.1912536308604868
+    z: 0.13099884470917866
+  orientation:
+    x: 1.0
+    y: 0.0
+    z: 0.0
+    w: 0.0
--- a/rbs_mill_assist/config/objects.yaml
+++ b/rbs_mill_assist/config/objects.yaml
@ -0,0 +1,14 @@
+table:
+  dimentions: [1.2, 0.7, 0.8]
+  pose:
+    position:
+      x: 0.0
+      y: 0.0
+      z: -0.4
+    orientation:
+      x: 0.0
+      y: 0.0
+      z: 0.0
+      w: 1.0
+laser_graver:
+  
--- a/rbs_mill_assist/include/rbs_mill_assist/VacuumGripper.hpp
+++ b/rbs_mill_assist/include/rbs_mill_assist/VacuumGripper.hpp
@ -8,7 +8,9 @@
 #include <gz/sim/System.hh>

 #include "std_srvs/srv/set_bool.hpp"
+#include "std_msgs/msg/bool.hpp"
 #include <memory>
+#include <rclcpp/publisher.hpp>
 #include <rclcpp/service.hpp>

 namespace rbs_mill_assist {
@ -17,7 +19,8 @@ class VacuumGripperPrivate;

 class VacuumGripper : public gz::sim::System,
                      public gz::sim::ISystemConfigure,
-                      public gz::sim::ISystemPreUpdate {
+                      public gz::sim::ISystemPreUpdate,
+                      public gz::sim::ISystemPostUpdate {
 public:
  VacuumGripper();
  ~VacuumGripper() override;
@ -29,6 +32,9 @@ public:
  void PreUpdate(const gz::sim::UpdateInfo &_info,
                 gz::sim::EntityComponentManager &_ecm) override;

+  void PostUpdate(const gz::sim::UpdateInfo &_info,
+                  const gz::sim::EntityComponentManager &_ecm) override;
+
  bool AttachLink(gz::sim::EntityComponentManager &_ecm,
                  const gz::sim::Entity &gripperEntity,
                  const gz::sim::Entity &objectEntity);
@ -41,6 +47,7 @@ public:
 private:
  std::unique_ptr<VacuumGripperPrivate> dataPtr;
  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_service_;
+  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr contact_publisher_;
 };
 } // namespace rbs_mill_assist
 #endif
--- a/rbs_mill_assist/launch/moveit.launch.py
+++ b/rbs_mill_assist/launch/moveit.launch.py
@ -0,0 +1,153 @@
+from launch_ros.actions import Node
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import (
+    LaunchConfiguration,
+)
+from moveit_configs_utils import MoveItConfigsBuilder, moveit_configs_builder
+
+
+def generate_launch_description():
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_description",
+            default_value="''",
+            description="robot description param",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_description_semantic",
+            default_value="''",
+            description="robot description semantic param",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_description_kinematics",
+            default_value="''",
+            description="robot description kinematics param",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "moveit_config_package",
+            default_value="rbs_arm",
+            description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
+        is not set, it enables use of a custom moveit config.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "moveit_config_file",
+            default_value="rbs_arm.srdf.xacro",
+            description="MoveIt SRDF/XACRO description file with the robot.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_sim_time",
+            default_value="true",
+            description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "tf_prefix",
+            default_value="''",
+            description="tf_prefix for robot links",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "namespace",
+            default_value="",
+            description="Namespace for move_group_node",
+        )
+    )
+
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_setup)]
+    )
+
+
+def launch_setup(context, *args, **kwargs):
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    robot_description_content = LaunchConfiguration("robot_description").perform(
+        context
+    )
+    robot_description_semantic_content = LaunchConfiguration(
+        "robot_description_semantic"
+    ).perform(context)
+    namespace = LaunchConfiguration("namespace").perform(context)
+
+    robot_description = {"robot_description": robot_description_content}
+    robot_description_semantic = {
+        "robot_description_semantic": robot_description_semantic_content
+    }
+    use_sim_time = {"use_sim_time": use_sim_time}
+
+    moveit_config = (
+        MoveItConfigsBuilder(robot_name="rbs_arm", package_name="rbs_mill_assist")
+        .trajectory_execution(file_path="moveit/moveit_controllers.yaml")
+        .joint_limits("moveit/joint_limits.yaml")
+        .pilz_cartesian_limits("moveit/pilz_cartesian_limits.yaml")
+        .robot_description_kinematics("moveit/kinematics.yaml")
+        .robot_description_semantic("srdf/rbs_arm.srdf", mappings={})
+        .planning_pipelines(
+            pipelines=["ompl", "stomp", "chomp"], default_planning_pipeline="ompl"
+        )
+        .to_moveit_configs()
+    )
+    planners_ids = [
+        "RRTstar",
+        "PRM",
+        "BiTRRT",
+        "SPARStwo",
+    ]
+    (
+        moveit_config.planning_pipelines["ompl"]
+        .setdefault("arm", {})
+        # .setdefault("planner_configs", {})
+        .update(
+            {"planner_configs":planners_ids}
+        )
+    )
+
+    move_group_node = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        namespace=namespace,
+        parameters=[moveit_config.to_dict(), robot_description, use_sim_time],
+    )
+    planning_scene_publisher = Node(
+        package="rbs_mill_assist",
+        executable="planning_scene_publisher",
+        parameters=[moveit_config.to_dict(), {"use_sim_time": True}],
+    )
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        # arguments=["-d", rviz_full_config],
+        parameters=[
+            use_sim_time,
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.planning_pipelines,
+            moveit_config.planning_scene_monitor,
+            moveit_config.robot_description_kinematics,
+            moveit_config.joint_limits,
+        ],
+    )
+
+    nodes_to_start = [
+        move_group_node,
+        planning_scene_publisher,
+        rviz_node,
+    ]
+
+    return nodes_to_start
--- a/rbs_mill_assist/launch/simulation.launch.py
+++ b/rbs_mill_assist/launch/simulation.launch.py
@ -1,3 +1,4 @@
+from ntpath import isfile
 import os

 import xacro
@ -64,23 +65,20 @@ def launch_setup(context, *args, **kwargs):
    robot_description_semantic_content = ""

    if use_moveit.perform(context) == "true":
-        srdf_config_file = os.path.join(
-            get_package_share_directory(moveit_config_package.perform(context)),
-            "srdf",
-            "xacro_args.yaml",
-        )
-        srdf_file = os.path.join(
-            get_package_share_directory(moveit_config_package.perform(context)),
-            "srdf",
-            moveit_config_file.perform(context),
-        )
-        srdf_mappings = load_xacro_args(srdf_config_file, locals())
-        robot_description_semantic_content = xacro.process_file(
-            srdf_file, mappings=srdf_mappings
-        )
-        robot_description_semantic_content = (
-            robot_description_semantic_content.toprettyxml(indent=" ")
-        )
+        package_dir = get_package_share_directory(moveit_config_package.perform(context))
+        
+        srdf_file = os.path.join(package_dir, "srdf", moveit_config_file.perform(context))
+
+        if srdf_file.endswith(".xacro"):
+            srdf_config_file = os.path.join(package_dir, "srdf", "xacro_args.yaml")
+            srdf_mappings = load_xacro_args(srdf_config_file, locals())
+            robot_description_semantic_content = xacro.process_file(srdf_file, mappings=srdf_mappings)
+            robot_description_semantic_content = robot_description_semantic_content.toprettyxml(indent=" ")
+        
+        elif srdf_file.endswith(".srdf"):
+            with open(srdf_file, "r") as file:
+                robot_description_semantic_content = file.read()
+                
        control_space = "joint"
        control_strategy = "position"
        interactive = "false"
@ -143,7 +141,7 @@ def launch_setup(context, *args, **kwargs):
            PythonLaunchDescriptionSource(
                [os.path.join(get_package_share_directory('ros_gz_sim'),
                            'launch', 'gz_sim.launch.py')]),
-            launch_arguments=[('gz_args', [' -r -v 4 ', gazebo_world])],
+            launch_arguments=[('gz_args', [' -r ', gazebo_world])],
    )

    gazebo_spawn_robot = Node(
@ -170,13 +168,44 @@ def launch_setup(context, *args, **kwargs):
        executable="grasping_service.py"
    )

+    get_named_pose_service = Node(
+        package="rbs_mill_assist",
+        executable="get_key_pose_frame.py"
+    )
+
+    get_workspace = Node(
+        package="rbs_mill_assist",
+        executable="get_workspace",
+        parameters=[
+            {
+                "urdf_path": os.path.join(get_package_share_directory("rbs_mill_assist"), "urdf", "current.urdf"),
+                "ee_link": ee_link_name,
+                "use_sim_time": True,
+                "robot_position": [0.0, 0.0, 0.0]
+            }
+        ]
+    )
+
+    publish_ee_pose = Node(
+        package="rbs_mill_assist",
+        executable="publish_ee_pose_node",
+        parameters=[
+            {"use_sim_time": use_sim_time},
+            {"base_link": base_link_name},
+            {"ee_link": ee_link_name}
+        ]
+    )
+
    nodes_to_start = [
        rbs_robot_setup,
-        rviz_node,
+        # rviz_node,
        gazebo,
        gazebo_spawn_robot,
        gz_bridge,
-        grasping_service
+        grasping_service,
+        get_named_pose_service,
+        # publish_ee_pose
+        # get_workspace
    ]
    return nodes_to_start

@ -226,7 +255,7 @@ def generate_launch_description():
    declared_arguments.append(
        DeclareLaunchArgument(
            "moveit_config_package",
-            default_value="rbs_arm",
+            default_value="rbs_mill_assist",
            description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
        is not set, it enables use of a custom moveit config.",
        )
@ -234,7 +263,7 @@ def generate_launch_description():
    declared_arguments.append(
        DeclareLaunchArgument(
            "moveit_config_file",
-            default_value="rbs_arm.srdf.xacro",
+            default_value="rbs_arm.srdf",
            description="MoveIt SRDF/XACRO description file with the robot.",
        )
    )
@ -253,7 +282,7 @@ def generate_launch_description():
    )
    declared_arguments.append(
        DeclareLaunchArgument(
-            "use_moveit", default_value="false", description="Launch moveit?"
+            "use_moveit", default_value="true", description="Launch moveit?"
        )
    )
    declared_arguments.append(
@ -308,7 +337,7 @@ def generate_launch_description():
    declared_arguments.append(
        DeclareLaunchArgument(
            "interactive",
-            default_value="false",
+            default_value="true",
            description="Wheter to run the motion_control_handle controller",
        ),
    )
--- a/rbs_mill_assist/meshes/Link6.STL
+++ b/rbs_mill_assist/meshes/Link6.STL
--- a/rbs_mill_assist/meshes/Link7.STL
+++ b/rbs_mill_assist/meshes/Link7.STL
--- a/rbs_mill_assist/meshes/Link8.STL
+++ b/rbs_mill_assist/meshes/Link8.STL
--- a/rbs_mill_assist/moveit/chomp_planning.yaml
+++ b/rbs_mill_assist/moveit/chomp_planning.yaml
@ -0,0 +1,37 @@
+planning_plugins:
+  - chomp_interface/CHOMPPlanner
+request_adapters:
+  - default_planning_request_adapters/ResolveConstraintFrames
+  - default_planning_request_adapters/ValidateWorkspaceBounds
+  - default_planning_request_adapters/CheckStartStateBounds
+  - default_planning_request_adapters/CheckStartStateCollision
+response_adapters:
+  - default_planning_response_adapters/AddTimeOptimalParameterization
+planning_time_limit: 10.0
+max_iterations: 200
+max_iterations_after_collision_free: 5
+smoothness_cost_weight: 0.1
+obstacle_cost_weight: 1.0
+learning_rate: 0.01
+animate_path: true
+add_randomness: false
+smoothness_cost_velocity: 0.0
+smoothness_cost_acceleration: 1.0
+smoothness_cost_jerk: 0.0
+hmc_discretization: 0.01
+hmc_stochasticity: 0.01
+hmc_annealing_factor: 0.99
+use_hamiltonian_monte_carlo: false
+ridge_factor: 0.0
+use_pseudo_inverse: false
+pseudo_inverse_ridge_factor: 1e-4
+animate_endeffector: false
+# animate_endeffector_segment: "panda_rightfinger"
+joint_update_limit: 0.1
+collision_clearance: 0.2
+collision_threshold: 0.07
+random_jump_amount: 1.0
+use_stochastic_descent: true
+enable_failure_recovery: false
+max_recovery_attempts: 5
+trajectory_initialization_method: "quintic-spline"
--- a/rbs_mill_assist/moveit/initial_positions.yaml
+++ b/rbs_mill_assist/moveit/initial_positions.yaml
@ -0,0 +1,9 @@
+# Default initial positions for rbs_arm's ros2_control fake system
+
+initial_positions:
+  ax1: 0
+  ax2: 0
+  ax3: 0
+  ax4: 0
+  ax5: 0
+  ax6: 0
--- a/rbs_mill_assist/moveit/joint_limits.yaml
+++ b/rbs_mill_assist/moveit/joint_limits.yaml
@ -0,0 +1,40 @@
+# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+
+# For beginners, we downscale velocity and acceleration limits.
+# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
+default_velocity_scaling_factor: 0.1
+default_acceleration_scaling_factor: 0.1
+
+# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+joint_limits:
+  ax1:
+    has_velocity_limits: true
+    max_velocity: 10.0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
+  ax2:
+    has_velocity_limits: true
+    max_velocity: 10.0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
+  ax3:
+    has_velocity_limits: true
+    max_velocity: 10.0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
+  ax4:
+    has_velocity_limits: true
+    max_velocity: 10.0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
+  ax5:
+    has_velocity_limits: true
+    max_velocity: 10.0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
+  ax6:
+    has_velocity_limits: true
+    max_velocity: 10.0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
--- a/rbs_mill_assist/moveit/kinematics.yaml
+++ b/rbs_mill_assist/moveit/kinematics.yaml
@ -0,0 +1,4 @@
+arm:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.0050000000000000001
+  kinematics_solver_timeout: 0.0050000000000000001
--- a/rbs_mill_assist/moveit/moveit_controllers.yaml
+++ b/rbs_mill_assist/moveit/moveit_controllers.yaml
@ -0,0 +1,22 @@
+trajectory_execution:
+  "moveit_manage_controllers": False,
+  "trajectory_execution.allowed_execution_duration_scaling": 1.2,
+  "trajectory_execution.allowed_goal_duration_margin": 0.5,
+  "trajectory_execution.allowed_start_tolerance": 0.01,
+
+moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+
+moveit_simple_controller_manager:
+  controller_names:
+    - joint_trajectory_controller
+
+  joint_trajectory_controller:
+    type: FollowJointTrajectory
+    action_ns: follow_joint_trajectory
+    joints:
+      - ax1
+      - ax2
+      - ax3
+      - ax4
+      - ax5
+      - ax6
--- a/rbs_mill_assist/moveit/moveit_cpp.yaml
+++ b/rbs_mill_assist/moveit/moveit_cpp.yaml
@ -0,0 +1,43 @@
+planning_scene_monitor_options:
+  name: "planning_scene_monitor"
+  robot_description: "robot_description"
+  joint_state_topic: "/joint_states"
+  attached_collision_object_topic: "/planning_scene_monitor"
+  publish_planning_scene_topic: "/publish_planning_scene"
+  monitored_planning_scene_topic: "/monitored_planning_scene"
+  wait_for_initial_state_timeout: 10.0
+
+planning_pipelines:
+  pipeline_names: ["ompl", "chomp"]
+
+plan_request_params:
+  planning_attempts: 1
+  planning_pipeline: ompl
+  max_velocity_scaling_factor: 1.0
+  max_acceleration_scaling_factor: 1.0
+
+ompl_rrtc:  # Namespace for individual plan request
+  plan_request_params:  # PlanRequestParameters similar to the ones that are used by the single pipeline planning of moveit_cpp
+    planning_attempts: 1  # Number of attempts the planning pipeline tries to solve a given motion planning problem
+    planning_pipeline: ompl  # Name of the pipeline that is being used
+    planner_id: "RRTConnectkConfigDefault"  # Name of the specific planner to be used by the pipeline
+    max_velocity_scaling_factor: 1.0  # Velocity scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
+    max_acceleration_scaling_factor: 1.0  # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
+    planning_time: 1.0  # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned
+
+pilz_lin:
+  plan_request_params:
+    planning_attempts: 1
+    planning_pipeline: pilz_industrial_motion_planner
+    planner_id: "PTP"
+    max_velocity_scaling_factor: 1.0
+    max_acceleration_scaling_factor: 1.0
+    planning_time: 0.8
+
+chomp_planner:
+  plan_request_params:
+    planning_attempts: 1
+    planning_pipeline: chomp
+    max_velocity_scaling_factor: 1.0
+    max_acceleration_scaling_factor: 1.0
+    planning_time: 1.5
--- a/rbs_mill_assist/moveit/ompl_planning.yaml
+++ b/rbs_mill_assist/moveit/ompl_planning.yaml
@ -0,0 +1,94 @@
+planning_plugins:
+  - ompl_interface/OMPLPlanner
+request_adapters:
+  - default_planning_request_adapters/ResolveConstraintFrames
+  - default_planning_request_adapters/ValidateWorkspaceBounds
+  - default_planning_request_adapters/CheckStartStateBounds
+  - default_planning_request_adapters/CheckStartStateCollision
+response_adapters:
+  - default_planning_response_adapters/AddTimeOptimalParameterization
+  - default_planning_response_adapters/ValidateSolution
+  - default_planning_response_adapters/DisplayMotionPath
+planner_configs:
+  SBLkConfigDefault:
+    type: geometric::SBL
+    range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  ESTkConfigDefault:
+    type: geometric::EST
+    range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
+    goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
+  LBKPIECEkConfigDefault:
+    type: geometric::LBKPIECE
+    range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
+    min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
+  BKPIECEkConfigDefault:
+    type: geometric::BKPIECE
+    range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
+    failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
+    min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
+  KPIECEkConfigDefault:
+    type: geometric::KPIECE
+    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
+    border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.]
+    failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
+    min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
+  RRTkConfigDefault:
+    type: geometric::RRT
+    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
+    range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  RRTstarkConfigDefault:
+    type: geometric::RRTstar
+    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
+    delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1
+  TRRTkConfigDefault:
+    type: geometric::TRRT
+    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
+    max_states_failed: 10 # when to start increasing temp. default: 10
+    temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0
+    min_temperature: 10e-10 # lower limit of temp change. default: 10e-10
+    init_temperature: 10e-6 # initial temperature. default: 10e-6
+    frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
+    frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
+    k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup()
+  PRMkConfigDefault:
+    type: geometric::PRM
+    max_nearest_neighbors: 10 # use k nearest neighbors. default: 10
+  PRMstarkConfigDefault:
+    type: geometric::PRMstar
+ar_manipulator:
+  planner_configs:
+    - SBLkConfigDefault
+    - ESTkConfigDefault
+    - LBKPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - KPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - RRTstarkConfigDefault
+    - TRRTkConfigDefault
+    - PRMkConfigDefault
+    - PRMstarkConfigDefault
+  ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE
+  #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint)
+  longest_valid_segment_fraction: 0.01
+rbs_arm:
+  planner_configs:
+    - SBLkConfigDefault
+    - ESTkConfigDefault
+    - LBKPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - KPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - RRTstarkConfigDefault
+    - TRRTkConfigDefault
+    - PRMkConfigDefault
+    - PRMstarkConfigDefault
--- a/rbs_mill_assist/moveit/pilz_cartesian_limits.yaml
+++ b/rbs_mill_assist/moveit/pilz_cartesian_limits.yaml
@ -0,0 +1,6 @@
+# Limits for the Pilz planner
+cartesian_limits:
+  max_trans_vel: 1.0
+  max_trans_acc: 2.25
+  max_trans_dec: -5.0
+  max_rot_vel: 1.57
--- a/rbs_mill_assist/moveit/rbs_arm.ros2_control.xacro
+++ b/rbs_mill_assist/moveit/rbs_arm.ros2_control.xacro
@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="rbs_arm_ros2_control" params="name initial_positions_file">
+        <xacro:property name="initial_positions" value="${xacro.load_yaml(initial_positions_file)['initial_positions']}"/>
+
+        <ros2_control name="${name}" type="system">
+            <hardware>
+                <!-- By default, set up controllers for simulation. This won't work on real hardware -->
+                <plugin>mock_components/GenericSystem</plugin>
+            </hardware>
+            <joint name="ax1">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['ax1']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="ax2">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['ax2']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="ax3">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['ax3']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="ax4">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['ax4']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="ax5">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['ax5']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="ax6">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['ax6']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+
+        </ros2_control>
+    </xacro:macro>
+</robot>
--- a/rbs_mill_assist/moveit/rbs_arm.urdf.xacro
+++ b/rbs_mill_assist/moveit/rbs_arm.urdf.xacro
@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rbs_arm">
+    <xacro:arg name="initial_positions_file" default="initial_positions.yaml" />
+
+    <!-- Import rbs_arm urdf file -->
+    <xacro:include filename="$(find rbs_mill_assist)/urdf/current.urdf" />
+
+    <!-- Import control_xacro -->
+    <xacro:include filename="rbs_arm.ros2_control.xacro" />
+
+
+    <xacro:rbs_arm_ros2_control name="FakeSystem" initial_positions_file="$(arg initial_positions_file)"/>
+
+</robot>
--- a/rbs_mill_assist/moveit/sensors_3d.yaml
+++ b/rbs_mill_assist/moveit/sensors_3d.yaml
@ -0,0 +1,11 @@
+sensors:
+  - default_sensor
+default_sensor:
+  filtered_cloud_topic: ""
+  max_range: ""
+  max_update_rate: ""
+  padding_offset: ""
+  padding_scale: ""
+  point_cloud_topic: ""
+  point_subsample: ""
+  sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
--- a/rbs_mill_assist/moveit/stomp_planning.yaml
+++ b/rbs_mill_assist/moveit/stomp_planning.yaml
--- a/rbs_mill_assist/package.xml
+++ b/rbs_mill_assist/package.xml
@ -20,12 +20,14 @@
  <depend>cartesian_motion_controller</depend>
  <depend>cartesian_twist_controller</depend>
  <depend>rbs_bt_executor</depend>
+  <depend>pinocchio</depend>


  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
+    <rviz plugin="${prefix}/materials/plugins.xml"/>
    <build_type>ament_cmake</build_type>
  </export>
 </package>
--- a/rbs_mill_assist/scripts/CMakeLists.txt
+++ b/rbs_mill_assist/scripts/CMakeLists.txt
@ -1,4 +1,5 @@
 install(PROGRAMS
  grasping_service.py
+  get_key_pose_frame.py
  DESTINATION lib/${PROJECT_NAME}
 )
--- a/rbs_mill_assist/scripts/get_key_pose_frame.py
+++ b/rbs_mill_assist/scripts/get_key_pose_frame.py
@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+
+import os
+from typing import Dict
+
+import rclpy
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from geometry_msgs.msg import Pose
+from rbs_utils_interfaces.srv import GetNamedPose
+from rclpy.node import Node
+from rclpy.service import Service
+
+
+class GetNamedPoseService(Node):
+    def __init__(self) -> None:
+        super().__init__("get_named_pose_service")
+        self.srv: Service = self.create_service(
+            GetNamedPose, "get_named_pose", self.get_named_pose
+        )
+        yaml_file_path: str = os.path.join(
+            get_package_share_directory("rbs_mill_assist"), "config", "key_poses.yaml"
+        )
+        with open(yaml_file_path, "r", encoding="utf-8") as file:
+            self.key_poses: Dict = yaml.safe_load(file)
+
+    def create_pose(self, pose_data: Dict) -> Pose:
+        """
+        Helper function to create a Pose from the given pose data.
+        """
+        pose = Pose()
+        if pose_data:
+            pose.position.x = pose_data.get("position", {}).get("x", 0.0)
+            pose.position.y = pose_data.get("position", {}).get("y", 0.0)
+            pose.position.z = pose_data.get("position", {}).get("z", 0.0)
+            pose.orientation.x = pose_data.get("orientation", {}).get("x", 0.0)
+            pose.orientation.y = pose_data.get("orientation", {}).get("y", 0.0)
+            pose.orientation.z = pose_data.get("orientation", {}).get("z", 0.0)
+            pose.orientation.w = pose_data.get("orientation", {}).get("w", 1.0)
+        return pose
+
+    def get_named_pose(
+        self, request: GetNamedPose.Request, response: GetNamedPose.Response
+    ) -> GetNamedPose.Response:
+
+        key_pose = self.key_poses.get(request.pose_name, None)
+        if key_pose is None:
+            response.ok = False
+            return response
+
+        response.named_pose.name = request.pose_name
+        response.named_pose.pose = self.create_pose(key_pose)
+        response.ok = True
+
+        return response
+
+
+def main():
+    rclpy.init()
+    executor = rclpy.executors.SingleThreadedExecutor()
+    # executor = rclpy.executors.MultiThreadedExecutor() # can't be used
+    i_node = GetNamedPoseService()
+    executor.add_node(i_node)
+    try:
+        executor.spin()
+    except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
+        i_node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
--- a/rbs_mill_assist/scripts/grasping_service.py
+++ b/rbs_mill_assist/scripts/grasping_service.py
@ -42,10 +42,11 @@ class GetGraspPickPoses(Node):
    def get_grasp_pick_poses(
        self, request: GetPickPlacePoses.Request, response: GetPickPlacePoses.Response
    ) -> GetPickPlacePoses.Response:
-        if request.action_name == "pick":
-            pregrasp_pose = self.grasping.get("pregrasp_pose", None)
-            grasp_pose = self.grasping.get("grasp_pose", None)
-            postgrasp_pose = self.grasping.get("postgrasp_pose", None)
+        if request.action_name == "to_graver.pick":
+            to_graver = self.grasping.get("to_graver", None)
+            pregrasp_pose = to_graver.get("pregrasp_pose", None)
+            grasp_pose = to_graver.get("grasp_pose", None)
+            postgrasp_pose = to_graver.get("postgrasp_pose", None)

            if None in [pregrasp_pose, grasp_pose, postgrasp_pose]:
                response.ok = False
@ -60,11 +61,49 @@ class GetGraspPickPoses(Node):
                self.create_pose(grasp_pose),
                self.create_pose(postgrasp_pose),
            ]
+        elif request.action_name == "to_graver.place":
+            to_graver = self.grasping.get("to_graver", None)
+            preplace_pose = to_graver.get("preplace_pose", None)
+            place_pose = to_graver.get("place_pose", None)
+            postplace_pose = to_graver.get("postplace_pose", None)

-        elif request.action_name == "place":
-            preplace_pose = self.grasping.get("preplace_pose", None)
-            place_pose = self.grasping.get("place_pose", None)
-            postplace_pose = self.grasping.get("postplace_pose", None)
+            if None in [preplace_pose, place_pose, postplace_pose]:
+                response.ok = False
+                self.get_logger().error(
+                    "Missing one or more of the preplace, place, or postplace poses."
+                )
+                return response
+
+            # Create Pose messages for placing
+            response.place = [
+                self.create_pose(preplace_pose),
+                self.create_pose(place_pose),
+                self.create_pose(postplace_pose),
+            ]
+        elif request.action_name == "from_graver.pick":
+            from_graver = self.grasping.get("from_graver", None)
+            pregrasp_pose = from_graver.get("pregrasp_pose", None)
+            grasp_pose = from_graver.get("grasp_pose", None)
+            postgrasp_pose = from_graver.get("postgrasp_pose", None)
+
+            if None in [pregrasp_pose, grasp_pose, postgrasp_pose]:
+                response.ok = False
+                self.get_logger().error(
+                    "Missing one or more of the pregrasp, grasp, or postgrasp poses."
+                )
+                return response
+
+            # Create Pose messages
+            response.grasp = [
+                self.create_pose(pregrasp_pose),
+                self.create_pose(grasp_pose),
+                self.create_pose(postgrasp_pose),
+            ]
+        elif request.action_name == "from_graver.place":
+            from_graver = self.grasping.get("from_graver", None)
+            preplace_pose = from_graver.get("preplace_pose", None)
+            place_pose = from_graver.get("place_pose", None)
+            postplace_pose = from_graver.get("postplace_pose", None)

            if None in [preplace_pose, place_pose, postplace_pose]:
                response.ok = False
--- a/rbs_mill_assist/src/CMakeLists.txt
+++ b/rbs_mill_assist/src/CMakeLists.txt
@ -0,0 +1,95 @@
+# Gazebo VacuumGripper plugin
+find_package(rclcpp REQUIRED)
+find_package(gz-cmake3 REQUIRED)
+find_package(gz-plugin2 REQUIRED COMPONENTS register)
+find_package(std_srvs REQUIRED)
+find_package(std_msgs REQUIRED)
+set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
+find_package(gz-common5 REQUIRED COMPONENTS profiler)
+set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR})
+
+# Harmonic
+if("$ENV{GZ_VERSION}" STREQUAL "harmonic")
+  find_package(gz-sim8 REQUIRED)
+  set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
+  message(STATUS "Compiling against Gazebo Harmonic")
+  # Default to Garden
+else()
+  find_package(gz-sim7 REQUIRED)
+  set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR})
+  message(STATUS "Compiling against Gazebo Garden")
+endif()
+
+add_library(VacuumGripper
+  SHARED
+  VacuumGripper.cpp
+)
+
+target_link_libraries(VacuumGripper
+  gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
+)
+
+ament_target_dependencies(VacuumGripper
+  rclcpp
+  std_srvs
+  std_msgs
+)
+
+install(
+  TARGETS VacuumGripper
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# GetWorkspace Node
+
+find_package(sensor_msgs REQUIRED)
+find_package(pinocchio REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+add_executable(get_workspace get_workspace.cpp)
+ament_target_dependencies(get_workspace rclcpp sensor_msgs pinocchio Eigen3)
+target_include_directories(get_workspace PRIVATE ${Eigen3_INCLUDE_DIRS})
+target_link_libraries(get_workspace Eigen3::Eigen pinocchio::pinocchio)
+
+install(
+  TARGETS get_workspace
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# planning_scene_publisher
+
+find_package(moveit_msgs REQUIRED)
+find_package(moveit_core REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(shape_msgs REQUIRED)
+
+add_executable(planning_scene_publisher planning_scene_publisher.cpp)
+ament_target_dependencies(planning_scene_publisher moveit_msgs moveit_core geometry_msgs)
+
+install(
+  TARGETS planning_scene_publisher
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# publish_ee_pose
+
+find_package(rclcpp_components REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+
+add_library(publish_ee_pose SHARED publish_ee_pose.cpp)
+target_include_directories(publish_ee_pose
+  PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+          $<INSTALL_INTERFACE:include>)
+target_compile_definitions(publish_ee_pose PRIVATE "PUBLISH_EE_POSE_BUILDING_DLL")
+ament_target_dependencies(publish_ee_pose rclcpp_components tf2_ros std_msgs tf2_geometry_msgs)
+rclcpp_components_register_node(publish_ee_pose PLUGIN "rbs_mill_assist::PublishEePose" EXECUTABLE publish_ee_pose_node)
+
+install(
+  TARGETS
+          publish_ee_pose
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME})
+
--- a/rbs_mill_assist/src/VacuumGripper.cpp
+++ b/rbs_mill_assist/src/VacuumGripper.cpp
@ -1,4 +1,5 @@
 #include "gz/sim/components/Name.hh"
+#include "std_msgs/msg/bool.hpp"
 #include <functional>
 #include <gz/common/Console.hh>
 #include <gz/plugin/Register.hh>
@ -21,7 +22,8 @@

 GZ_ADD_PLUGIN(rbs_mill_assist::VacuumGripper, gz::sim::System,
              rbs_mill_assist::VacuumGripper::ISystemConfigure,
-              rbs_mill_assist::VacuumGripper::ISystemPreUpdate)
+              rbs_mill_assist::VacuumGripper::ISystemPreUpdate,
+              rbs_mill_assist::VacuumGripper::ISystemPostUpdate)

 namespace rbs_mill_assist {

@ -31,6 +33,7 @@ public:
  bool attach_request;
  bool detach_request;
  bool enabled;
+  bool attached;
  gz::sim::EntityComponentManager _ecm;

  gz::sim::components::Joint joint;
@ -88,6 +91,7 @@ bool VacuumGripper::AttachLink(gz::sim::EntityComponentManager &_ecm,
        << std::endl;

  this->dataPtr->contactedEntity = objectEntity;
+  this->dataPtr->attached = true;

  return true;
 }
@ -113,6 +117,7 @@ bool VacuumGripper::DetachLink(gz::sim::EntityComponentManager &_ecm) {

  this->dataPtr->contactedEntity = gz::sim::kNullEntity;
  this->dataPtr->detachableJoints.erase(it);
+  this->dataPtr->attached = false;

  return true;
 }
@ -180,6 +185,10 @@ void VacuumGripper::Configure(
          std::bind(&VacuumGripper::Toggle, this, std::placeholders::_1,
                    std::placeholders::_2));

+  contact_publisher_ =
+      this->dataPtr->node_->create_publisher<std_msgs::msg::Bool>(
+          node_name + "/contact_info", 10);
+
  gzdbg << "rbs_mill_assist::VacuumGripper::Configure on entity: " << _entity
        << std::endl;
 }
@ -234,4 +243,11 @@ void VacuumGripper::PreUpdate(const gz::sim::UpdateInfo &_info,
      });
 }

+void VacuumGripper::PostUpdate(const gz::sim::UpdateInfo &_info,
+                               const gz::sim::EntityComponentManager &_ecm) {
+  std_msgs::msg::Bool msg;
+  msg.data = this->dataPtr->attached;
+  contact_publisher_->publish(msg);
+}
+
 } // namespace rbs_mill_assist
--- a/rbs_mill_assist/src/get_workspace.cpp
+++ b/rbs_mill_assist/src/get_workspace.cpp
@ -0,0 +1,202 @@
+#include <Eigen/Dense>
+#include <Eigen/src/Core/Matrix.h>
+#include <fstream>
+#include <memory>
+#include <pinocchio/algorithm/joint-configuration.hpp>
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/kinematics.hpp>
+#include <pinocchio/parsers/urdf.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/point_field.hpp>
+#include <string>
+#include <urdf_model/model.h>
+#include <urdf_parser/urdf_parser.h>
+#include <vector>
+
+using namespace std;
+using namespace pinocchio;
+using sensor_msgs::msg::PointCloud2;
+using sensor_msgs::msg::PointField;
+
+class WorkspaceCalculator : public rclcpp::Node {
+public:
+  WorkspaceCalculator() : Node("workspace_calculator") {
+    this->declare_parameter("urdf_path", "");
+    this->declare_parameter("ee_link", "");
+    this->declare_parameter("joint_resolution", 7);
+    this->declare_parameter("robot_position", std::vector<double>({0.0, 0.0, 0.0}));
+
+
+    robot_position = this->get_parameter("robot_position").as_double_array();
+    joint_resolution_ = this->get_parameter("joint_resolution").as_int();
+    ee_link_ = this->get_parameter("ee_link").as_string();
+    urdf_path_ = this->get_parameter("urdf_path").as_string();
+
+    publisher_ = this->create_publisher<PointCloud2>("workspace", 10);
+
+    compute_workspace();
+    timer_ = this->create_wall_timer(
+        std::chrono::seconds(1),
+        std::bind(&WorkspaceCalculator::publish_pointcloud, this));
+  }
+
+private:
+  rclcpp::Publisher<PointCloud2>::SharedPtr publisher_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  std::string urdf_path_;
+  std::string ee_link_;
+  std::vector<double> robot_position{};
+  int joint_resolution_;
+  std::vector<Eigen::Vector3d> workspace_points_;
+
+  void compute_workspace() {
+    if (urdf_path_.empty() || ee_link_.empty()) {
+      RCLCPP_ERROR(this->get_logger(),
+                   "URDF path and end-effector link must be provided.");
+      return;
+    }
+
+    Model model;
+    pinocchio::urdf::buildModel(urdf_path_, model);
+    Data data(model);
+    auto urdf_robot = ::urdf::parseURDFFile(urdf_path_);
+
+    auto joint_limits = get_joint_limits(urdf_robot, model);
+    auto joint_grid = create_joint_grid(joint_limits, joint_resolution_);
+
+
+
+    for (const Eigen::VectorXd &q : joint_grid) {
+      forwardKinematics(model, data, q);
+      updateFramePlacements(model, data);
+ 
+      auto ee_index = model.getFrameId(ee_link_);
+      if (ee_index < 0) {
+        RCLCPP_ERROR(this->get_logger(),
+                     "End-effector link not found in model.");
+      }
+
+
+      auto ee_pos = data.oMf[ee_index].translation();
+      workspace_points_.emplace_back(ee_pos);
+
+      if (ee_pos.isApprox(Eigen::Vector3d::Zero(), 1e-6)) {
+          continue; // Пропускаем нулевую позицию
+      }
+
+    }
+    RCLCPP_INFO(this->get_logger(), "Workspace calculation has been ended");
+  }
+
+  std::vector<std::pair<double, double>>
+  get_joint_limits(const std::shared_ptr<::urdf::ModelInterface> &urdf_robot,
+                   const Model &model) {
+    std::vector<std::pair<double, double>> joint_limits;
+
+    for (const auto &joint : urdf_robot->joints_) {
+      if (joint.second->limits) {
+        joint_limits.emplace_back(joint.second->limits->lower,
+                                  joint.second->limits->upper);
+      }
+    }
+
+    return joint_limits;
+  }
+
+  std::vector<Eigen::VectorXd>
+  create_joint_grid(const std::vector<std::pair<double, double>> &joint_limits,
+                    int resolution) {
+    std::vector<Eigen::VectorXd> grid;
+    std::vector<std::vector<double>> joint_values;
+
+    for (const auto &lim : joint_limits) {
+      std::vector<double> range;
+      double step = (lim.second - lim.first) / (resolution - 1);
+      for (int i = 0; i < resolution; ++i) {
+        range.push_back(lim.first + i * step);
+      }
+      joint_values.push_back(range);
+    }
+
+    size_t num_joints = joint_limits.size();
+    std::vector<size_t> indices(num_joints, 0);
+    while (true) {
+      Eigen::VectorXd q(num_joints);
+      for (size_t i = 0; i < num_joints; ++i) {
+        q[i] = joint_values[i][indices[i]];
+      }
+      grid.push_back(q);
+
+      for (size_t i = 0; i < num_joints; ++i) {
+        if (++indices[i] < resolution)
+          break;
+        if (i == num_joints - 1)
+          return grid;
+        indices[i] = 0;
+      }
+    }
+  }
+
+  void publish_pointcloud() {
+    if (workspace_points_.empty()) {
+      RCLCPP_ERROR(this->get_logger(), "No workspace points computed.");
+      return;
+    }
+
+    PointCloud2 msg;
+    msg.header.stamp = this->now();
+    msg.header.frame_id = "base_link";
+    msg.height = 1;
+    msg.width = workspace_points_.size();
+    msg.is_dense = false;
+    msg.is_bigendian = false;
+
+    sensor_msgs::msg::PointField field_x;
+    field_x.name = "x";
+    field_x.offset = 0;
+    field_x.datatype = sensor_msgs::msg::PointField::FLOAT32;
+    field_x.count = 1;
+    msg.fields.push_back(field_x);
+
+    sensor_msgs::msg::PointField field_y;
+    field_y.name = "y";
+    field_y.offset = 4;
+    field_y.datatype = sensor_msgs::msg::PointField::FLOAT32;
+    field_y.count = 1;
+    msg.fields.push_back(field_y);
+
+    sensor_msgs::msg::PointField field_z;
+    field_z.name = "z";
+    field_z.offset = 8;
+    field_z.datatype = sensor_msgs::msg::PointField::FLOAT32;
+    field_z.count = 1;
+    msg.fields.push_back(field_z);
+
+    msg.point_step = 12;
+    msg.row_step = msg.point_step * msg.width;
+    msg.data.resize(msg.row_step);
+
+    uint8_t *data_ptr = msg.data.data();
+    for (const auto &p : workspace_points_) {
+      // RCLCPP_INFO(this->get_logger(), "Point: (%f, %f, %f)", p.x(), p.y(),
+      //             p.z());
+      float x = p.x() - robot_position[0], y = p.y() - robot_position[1], z = p.z() - robot_position[2];
+      memcpy(data_ptr, &x, 4);
+      memcpy(data_ptr + 4, &y, 4);
+      memcpy(data_ptr + 8, &z, 4);
+      data_ptr += 12;
+    }
+
+    publisher_->publish(msg);
+  }
+};
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<WorkspaceCalculator>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}
--- a/rbs_mill_assist/src/planning_scene_publisher.cpp
+++ b/rbs_mill_assist/src/planning_scene_publisher.cpp
@ -0,0 +1,112 @@
+#include "moveit_msgs/msg/planning_scene.hpp"
+#include "shape_msgs/msg/mesh.hpp"
+#include <chrono>
+#include <functional>
+#include <geometry_msgs/msg/pose.hpp>
+#include <rclcpp/publisher.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <moveit_msgs/msg/collision_object.hpp>
+#include <moveit_msgs/msg/planning_scene.hpp>
+#include <shape_msgs/msg/solid_primitive.hpp>
+#include <shape_msgs/msg/mesh.hpp>
+#include <geometric_shapes/shape_operations.h>
+
+class PlanningScenePublisher : public rclcpp::Node {
+public:
+  PlanningScenePublisher() : Node("planning_scene_publisher"), object_published_(false) {
+    planning_scene_publisher_ =
+        this->create_publisher<moveit_msgs::msg::PlanningScene>(
+            "planning_scene", 10);
+    timer_ = this->create_wall_timer(
+        std::chrono::seconds(1),
+        std::bind(&PlanningScenePublisher::publish_scene, this));
+  }
+
+private:
+  rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr planning_scene_publisher_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  bool object_published_;
+
+  void publish_scene() {
+    if (object_published_) {
+      return; // Избегаем повторной публикации объекта
+    }
+    
+    moveit_msgs::msg::PlanningScene planning_scene;
+    moveit_msgs::msg::CollisionObject collision_object;
+    
+    collision_object.header.frame_id = "world";
+    collision_object.id = "table";
+    
+    // Определяем геометрию объекта (стола)
+    shape_msgs::msg::SolidPrimitive table;
+    table.type = shape_msgs::msg::SolidPrimitive::BOX;
+    table.dimensions = {1.2, 0.7, 0.8}; // Длина, ширина, высота
+
+    // Задаем позу объекта
+    geometry_msgs::msg::Pose table_pose;
+    table_pose.position.x = 0.0;
+    table_pose.position.y = 0.0;
+    table_pose.position.z = -0.4;
+    
+    collision_object.primitives.push_back(table);
+    collision_object.primitive_poses.push_back(table_pose);
+    collision_object.operation = moveit_msgs::msg::CollisionObject::ADD;
+
+    planning_scene.world.collision_objects.push_back(collision_object);
+    
+    // Добавляем объект меша
+    moveit_msgs::msg::CollisionObject mesh_object;
+    mesh_object.header.frame_id = "world";
+    mesh_object.id = "mesh_object";
+
+    shape_msgs::msg::Mesh mesh;
+    shapes::Mesh* m = shapes::createMeshFromResource("package://rbs_mill_assist/assets/laser/meshes/laser.dae");
+    shapes::ShapeMsg mesh_msg;
+    shapes::constructMsgFromShape(m, mesh_msg);
+    mesh = boost::get<shape_msgs::msg::Mesh>(mesh_msg);
+
+    //0.30 0.0 0 0 0 3.14159
+    geometry_msgs::msg::Pose mesh_pose;
+    mesh_pose.position.x = 0.30;
+    mesh_pose.position.y = 0.0;
+    mesh_pose.position.z = 0.0;
+
+    mesh_pose.orientation.x = 0.0;
+    mesh_pose.orientation.y = 0.0;
+    mesh_pose.orientation.z = 1.0;
+    mesh_pose.orientation.w = 0.0;
+
+    moveit_msgs::msg::ObjectColor color;
+    color.id = mesh_object.id;
+    color.color.r = 0.0;
+    color.color.g = 1.0;
+    color.color.b = 0.0;
+    color.color.a = 1.0;
+
+    
+    mesh_object.meshes.push_back(mesh);
+    mesh_object.mesh_poses.push_back(mesh_pose);
+    mesh_object.operation = moveit_msgs::msg::CollisionObject::ADD;
+
+    // planning_scene.object_colors.push_back(color);
+    // planning_scene.object_colors.clear();
+
+    planning_scene.world.collision_objects.push_back(mesh_object);
+    planning_scene.is_diff = true;
+    
+    planning_scene_publisher_->publish(planning_scene);
+    RCLCPP_INFO(this->get_logger(), "Published collision objects: table and mesh");
+    
+    object_published_ = true; // Устанавливаем флаг, чтобы больше не публиковать объект
+  }
+};
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<PlanningScenePublisher>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}
--- a/rbs_mill_assist/src/publish_ee_pose.cpp
+++ b/rbs_mill_assist/src/publish_ee_pose.cpp
@ -0,0 +1,67 @@
+#include "geometry_msgs/msg/pose.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/transform_listener.h"
+#include <chrono>
+#include <string>
+
+namespace rbs_mill_assist {
+class PublishEePose : public rclcpp::Node {
+public:
+  explicit PublishEePose(
+      const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
+      : Node("publish_ee_pose", options), tf_buffer_(this->get_clock()),
+        tf_listener_(tf_buffer_) {
+
+    this->declare_parameter<std::string>("ee_link", "ee_link");
+    this->declare_parameter<std::string>("base_link", "base_link");
+    this->get_parameter("ee_link", ee_link_);
+    this->get_parameter("base_link", base_link_);
+
+    publish_pose_ =
+        this->create_publisher<geometry_msgs::msg::Pose>("ee_pose", 10);
+
+    timer_ = this->create_wall_timer(
+        std::chrono::milliseconds(500),
+        std::bind(&PublishEePose::publishMessage, this));
+  }
+
+private:
+  void publishMessage() {
+    geometry_msgs::msg::TransformStamped transform_stamped;
+    try {
+      transform_stamped =
+          tf_buffer_.lookupTransform(base_link_, ee_link_, tf2::TimePointZero);
+
+      geometry_msgs::msg::Pose pose;
+      pose.position.x = transform_stamped.transform.translation.x;
+      pose.position.y = transform_stamped.transform.translation.y;
+      pose.position.z = transform_stamped.transform.translation.z;
+
+      pose.orientation = transform_stamped.transform.rotation;
+
+      // RCLCPP_INFO(this->get_logger(), "Publishing Pose: [x: %.2f, y: %.2f, z:
+      // %.2f]",
+      //             pose.position.x, pose.position.y, pose.position.z);
+
+      publish_pose_->publish(pose);
+    } catch (const tf2::TransformException &ex) {
+      RCLCPP_WARN(this->get_logger(), "Could not transform %s to %s: %s",
+                  ee_link_.c_str(), base_link_.c_str(), ex.what());
+    }
+  }
+
+  std::string ee_link_;
+  std::string base_link_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr publish_pose_;
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+};
+} // namespace rbs_mill_assist
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(rbs_mill_assist::PublishEePose);
--- a/rbs_mill_assist/srdf/rbs_arm.srdf
+++ b/rbs_mill_assist/srdf/rbs_arm.srdf
@ -0,0 +1,53 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<robot name="rbs_arm">
+    <group name="arm">
+        <chain base_link="base_link" tip_link="grasp_point"/>
+    </group>
+    <group_state name="zero" group="arm">
+        <joint name="ax1" value="0"/>
+        <joint name="ax2" value="0"/>
+        <joint name="ax3" value="0"/>
+        <joint name="ax4" value="0"/>
+        <joint name="ax5" value="0"/>
+        <joint name="ax6" value="0"/>
+    </group_state>
+
+    <group_state name="home" group="arm">
+        <joint name="ax1" value="1.57"/>
+        <joint name="ax2" value="0.6586"/>
+        <joint name="ax3" value="0.8582"/>
+        <joint name="ax4" value="0"/>
+        <joint name="ax5" value="1.5215"/>
+        <joint name="ax6" value="0"/>
+    </group_state>
+
+    <disable_collisions link1="Link1" link2="Link2" reason="Adjacent"/>
+    <disable_collisions link1="Link1" link2="base_link" reason="Adjacent"/>
+    <disable_collisions link1="Link2" link2="Link3" reason="Adjacent"/>
+    <disable_collisions link1="Link2" link2="Link5" reason="Never"/>
+    <disable_collisions link1="Link2" link2="base_link" reason="Never"/>
+    <disable_collisions link1="Link3" link2="Link4" reason="Adjacent"/>
+    <disable_collisions link1="Link3" link2="Link5" reason="Never"/>
+    <disable_collisions link1="Link3" link2="left_contact_panel" reason="Never"/>
+    <disable_collisions link1="Link4" link2="Link5" reason="Adjacent"/>
+    <disable_collisions link1="Link4" link2="Link6" reason="Never"/>
+    <disable_collisions link1="Link4" link2="Link7" reason="Never"/>
+    <disable_collisions link1="Link4" link2="Link8" reason="Never"/>
+    <disable_collisions link1="Link4" link2="left_contact_panel" reason="Never"/>
+    <disable_collisions link1="Link4" link2="right_contact_panel" reason="Never"/>
+    <disable_collisions link1="Link5" link2="Link6" reason="Adjacent"/>
+    <disable_collisions link1="Link5" link2="Link7" reason="Never"/>
+    <disable_collisions link1="Link5" link2="Link8" reason="Never"/>
+    <disable_collisions link1="Link5" link2="left_contact_panel" reason="Never"/>
+    <disable_collisions link1="Link5" link2="right_contact_panel" reason="Never"/>
+    <disable_collisions link1="Link6" link2="Link7" reason="Adjacent"/>
+    <disable_collisions link1="Link6" link2="Link8" reason="Adjacent"/>
+    <disable_collisions link1="Link6" link2="left_contact_panel" reason="Never"/>
+    <disable_collisions link1="Link6" link2="right_contact_panel" reason="Never"/>
+    <disable_collisions link1="Link7" link2="Link8" reason="Never"/>
+    <disable_collisions link1="Link7" link2="left_contact_panel" reason="Adjacent"/>
+    <disable_collisions link1="Link7" link2="right_contact_panel" reason="Never"/>
+    <disable_collisions link1="Link8" link2="left_contact_panel" reason="Never"/>
+    <disable_collisions link1="Link8" link2="right_contact_panel" reason="Adjacent"/>
+    <disable_collisions link1="left_contact_panel" link2="right_contact_panel" reason="Never"/>
+</robot>
--- a/rbs_mill_assist/urdf/current.urdf
+++ b/rbs_mill_assist/urdf/current.urdf
@ -5,32 +5,8 @@
 <!-- =================================================================================== -->
 <robot name="rbs_arm">
  <link name="world"/>
-  <link name="machine_link">
-    <visual>
-      <geometry>
-        <box size="0.4 0.5 0.115"/>
-      </geometry>
-      <material name="gray">
-        <color rgba="0 1 0 1"/>
-      </material>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.4 0.5 0.115"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="40.0"/>
-      <inertia ixx="0.6580625" ixy="0" ixz="0" iyy="0.4330625" iyz="0" izz="1.0250000000000001"/>
-    </inertial>
-  </link>
-  <joint name="machine_joint" type="fixed">
-    <parent link="world"/>
-    <child link="machine_link"/>
-    <origin rpy="0 0 0" xyz="0.25 0 0.0575"/>
-  </joint>
  <joint name="parent_connection" type="fixed">
-    <origin rpy="0.0 0.0 0.0" xyz="-0.10 0.0 0.03"/>
+    <origin rpy="0.0 0.0 0.0" xyz="-0.20 0.0 0.0"/>
    <parent link="world"/>
    <child link="base_link"/>
  </joint>
@ -83,7 +59,7 @@
    <parent link="base_link"/>
    <child link="Link1"/>
    <axis xyz="0 0 1"/>
-    <limit effort="30" lower="-3.14159" upper="3.14159" velocity="10"/>
+    <limit effort="30" lower="-6.2832" upper="6.2832" velocity="10"/>
  </joint>
  <link name="Link2">
    <inertial>
@ -111,8 +87,8 @@
    <origin rpy="0 0 0" xyz="0 0 0.15465"/>
    <parent link="Link1"/>
    <child link="Link2"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="30" lower="-1.57" upper="1.57" velocity="10"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="30" lower="-2.2689" upper="1.7453" velocity="10"/>
  </joint>
  <link name="Link3">
    <inertial>
@ -141,7 +117,7 @@
    <parent link="Link2"/>
    <child link="Link3"/>
    <axis xyz="1 0 0"/>
-    <limit effort="30" lower="-1.57" upper="1.57" velocity="10"/>
+    <limit effort="30" lower="-2.2689" upper="1.7453" velocity="10"/>
  </joint>
  <link name="Link4">
    <inertial>
@ -169,8 +145,8 @@
    <origin rpy="0 0 0" xyz="0 0 0.098"/>
    <parent link="Link3"/>
    <child link="Link4"/>
-    <axis xyz="0 0 -1"/>
-    <limit effort="30" lower="-3.14159" upper="3.14159" velocity="10"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="30" lower="-6.2832" upper="6.2832" velocity="10"/>
  </joint>
  <link name="Link5">
    <inertial>
@ -199,13 +175,13 @@
    <parent link="Link4"/>
    <child link="Link5"/>
    <axis xyz="1 0 0"/>
-    <limit effort="30" lower="-1.57" upper="1.57" velocity="10"/>
+    <limit effort="30" lower="-1.7453" upper="2.0944" velocity="10"/>
  </joint>
  <link name="Link6">
    <inertial>
-      <origin rpy="0 0 0" xyz="2.3821E-11 -0.054292 0.018171"/>
-      <mass value="0.41222"/>
-      <inertia ixx="0.00057892" ixy="-5.0591E-13" ixz="1.1462E-13" iyy="8.7903E-05" iyz="5.3383E-05" izz="0.00063814"/>
+      <origin rpy="0 0 0" xyz="1.11237115621161E-06 0.105319882001022 0.0151262038950002"/>
+      <mass value="0.280654829111196"/>
+      <inertia ixx="0.00166005184865464" ixy="-2.80959481690827E-08" ixz="-7.84445697318557E-10" iyy="2.87495064989592E-05" iyz="-7.18334236975057E-05" izz="0.00166929050771312"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
@ -213,7 +189,7 @@
        <mesh filename="file:///home/narenmak/rbs_ws/install/rbs_mill_assist/share/rbs_mill_assist/meshes/Link6.STL"/>
      </geometry>
      <material name="">
-        <color rgba="0.75294 0.75294 0.75294 1"/>
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
      </material>
    </visual>
    <collision>
@ -228,13 +204,13 @@
    <parent link="Link5"/>
    <child link="Link6"/>
    <axis xyz="0 0 1"/>
-    <limit effort="30" lower="-3.14159" upper="3.14159" velocity="10"/>
+    <limit effort="30" lower="-6.2832" upper="6.2832" velocity="10"/>
  </joint>
  <link name="Link7">
    <inertial>
-      <origin rpy="0 0 0" xyz="3.46944695195361E-18 -5.55111512312578E-17 -0.00360613836994461"/>
+      <origin rpy="0 0 0" xyz="8.67361737988404E-18 0 -0.0036061383699445"/>
      <mass value="0.0198935071603267"/>
-      <inertia ixx="1.01905343575873E-06" ixy="3.76158192263132E-37" ixz="-3.1449831265618E-23" iyy="1.01905343575873E-06" iyz="6.29843212132371E-23" izz="1.43041044988815E-06"/>
+      <inertia ixx="1.01905343575873E-06" ixy="1.40555551021685E-36" ixz="-7.39766847562968E-22" iyy="1.01905343575873E-06" iyz="-2.63361925044231E-22" izz="1.43041044988815E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
@ -253,16 +229,16 @@
    </collision>
  </link>
  <joint name="Right_point" type="fixed">
-    <origin rpy="0 0 0" xyz="0.015 -0.095 0.05"/>
+    <origin rpy="0 0 0" xyz="0.015 0.195 0.05"/>
    <parent link="Link6"/>
    <child link="Link7"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="Link8">
    <inertial>
-      <origin rpy="0 0 0" xyz="0 -2.77555756156289E-17 -0.00360613836994461"/>
+      <origin rpy="0 0 0" xyz="1.04083408558608E-17 0 -0.0036061383699445"/>
      <mass value="0.0198935071603267"/>
-      <inertia ixx="1.01905343575873E-06" ixy="0" ixz="-2.89418945570099E-23" iyy="1.01905343575872E-06" iyz="-1.78775100697742E-23" izz="1.43041044988815E-06"/>
+      <inertia ixx="1.01905343575873E-06" ixy="1.10608362843633E-36" ixz="-7.54785908468325E-22" iyy="1.01905343575873E-06" iyz="-2.63361925044231E-22" izz="1.43041044988815E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
@ -281,11 +257,84 @@
    </collision>
  </link>
  <joint name="Left_point" type="fixed">
-    <origin rpy="0 0 0" xyz="-0.015 -0.095 0.05"/>
+    <origin rpy="0 0 0" xyz="-0.015 0.195 0.05"/>
    <parent link="Link6"/>
    <child link="Link8"/>
    <axis xyz="0 0 0"/>
  </joint>
+  <link name="grasp_point"/>
+  <joint name="grasp_point_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0 0.195 0.05"/>
+    <parent link="Link6"/>
+    <child link="grasp_point"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <link name="right_contact_panel">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.0198935071603267"/>
+      <inertia ixx="1.674370185994164e-07" ixy="0" ixz="0" iyy="1.674370185994164e-07" iyz="0" izz="3.315584526721117e-07"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.001"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 0.4"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="left_contact_panel">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.0198935071603267"/>
+      <inertia ixx="1.674370185994164e-07" ixy="0" ixz="0" iyy="1.674370185994164e-07" iyz="0" izz="3.315584526721117e-07"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.001"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 0.4"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_contact_panel_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="Link8"/>
+    <child link="right_contact_panel"/>
+  </joint>
+  <joint name="left_contact_panel_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="Link7"/>
+    <child link="left_contact_panel"/>
+  </joint>
+  <gazebo reference="Right_point">
+    <preserveFixedJoint>true</preserveFixedJoint>
+  </gazebo>
+  <gazebo reference="Left_point">
+    <preserveFixedJoint>true</preserveFixedJoint>
+  </gazebo>
+  <gazebo reference="right_contact_panel_joint">
+    <preserveFixedJoint>true</preserveFixedJoint>
+  </gazebo>
+  <gazebo reference="left_contact_panel_joint">
+    <preserveFixedJoint>true</preserveFixedJoint>
+  </gazebo>
  <ros2_control name="ax1_hardware_interface" type="actuator">
    <hardware>
      <plugin>gz_ros2_control/GazeboSimSystem</plugin>
@ -293,10 +342,6 @@
    <joint name="ax1">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
-      <!-- WARN When this active a robot falls down -->
-      <!-- <command_interface name="effort"/> -->
-      <!-- <param name="p">${p}</param> -->
-      <!-- <param name="d">${d}</param> -->
      <!-- <command_interface name="effort"/> -->
      <state_interface name="position">
        <param name="initial_value">0.0</param>
@ -312,13 +357,9 @@
    <joint name="ax2">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
-      <!-- WARN When this active a robot falls down -->
-      <!-- <command_interface name="effort"/> -->
-      <!-- <param name="p">${p}</param> -->
-      <!-- <param name="d">${d}</param> -->
      <!-- <command_interface name="effort"/> -->
      <state_interface name="position">
-        <param name="initial_value">0.0</param>
+        <param name="initial_value">0.85</param>
      </state_interface>
      <state_interface name="velocity"/>
      <!-- <state_interface name="acceleration"/> -->
@ -331,13 +372,9 @@
    <joint name="ax3">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
-      <!-- WARN When this active a robot falls down -->
-      <!-- <command_interface name="effort"/> -->
-      <!-- <param name="p">${p}</param> -->
-      <!-- <param name="d">${d}</param> -->
      <!-- <command_interface name="effort"/> -->
      <state_interface name="position">
-        <param name="initial_value">0.0</param>
+        <param name="initial_value">1.0</param>
      </state_interface>
      <state_interface name="velocity"/>
      <!-- <state_interface name="acceleration"/> -->
@ -350,10 +387,6 @@
    <joint name="ax4">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
-      <!-- WARN When this active a robot falls down -->
-      <!-- <command_interface name="effort"/> -->
-      <!-- <param name="p">${p}</param> -->
-      <!-- <param name="d">${d}</param> -->
      <!-- <command_interface name="effort"/> -->
      <state_interface name="position">
        <param name="initial_value">0.0</param>
@ -369,13 +402,9 @@
    <joint name="ax5">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
-      <!-- WARN When this active a robot falls down -->
-      <!-- <command_interface name="effort"/> -->
-      <!-- <param name="p">${p}</param> -->
-      <!-- <param name="d">${d}</param> -->
      <!-- <command_interface name="effort"/> -->
      <state_interface name="position">
-        <param name="initial_value">0.0</param>
+        <param name="initial_value">1.0</param>
      </state_interface>
      <state_interface name="velocity"/>
      <!-- <state_interface name="acceleration"/> -->
@ -388,10 +417,6 @@
    <joint name="ax6">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
-      <!-- WARN When this active a robot falls down -->
-      <!-- <command_interface name="effort"/> -->
-      <!-- <param name="p">${p}</param> -->
-      <!-- <param name="d">${d}</param> -->
      <!-- <command_interface name="effort"/> -->
      <state_interface name="position">
        <param name="initial_value">0.0</param>
@ -401,6 +426,54 @@
    </joint>
  </ros2_control>
  <!-- <xacro:fts link="tool0" name="fts_sensor" tf_prefix="${tf_prefix}"></xacro:fts> -->
+  <link name="rgbd_camera">
+    <inertial>
+      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.1"/>
+      <inertia ixx="0.000166667" ixy="0.0" ixz="0.0" iyy="0.000166667" iyz="0.0" izz="0.000166667"/>
+    </inertial>
+    <visual name="">
+      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.0 0.0 0.0 1.0"/>
+        <texture filename=""/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="rgbd_camera">
+    <sensor name="rgbd_camera" type="rgbd_camera">
+      <camera>
+        <horizontal_fov>1.047</horizontal_fov>
+        <image>
+          <width>1920</width>
+          <height>1080</height>
+        </image>
+        <clip>
+          <near>0.1</near>
+          <far>3</far>
+        </clip>
+      </camera>
+      <always_on>1</always_on>
+      <update_rate>30</update_rate>
+      <visualize>true</visualize>
+      <topic>rgbd_camera</topic>
+      <enable_metrics>true</enable_metrics>
+    </sensor>
+  </gazebo>
+  <joint name="rgbd_camera_to_parent" type="fixed">
+    <parent link="Link1"/>
+    <child link="rgbd_camera"/>
+    <origin rpy="0.0 0.0 -1.57" xyz="0.0 -0.035 0.03"/>
+  </joint>
  <gazebo>
    <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
      <parameters></parameters>
@ -409,13 +482,20 @@
      </ros>
    </plugin>
    <plugin filename="VacuumGripper" name="rbs_mill_assist::VacuumGripper">
-      <link_name>Link6</link_name>
+      <link_name>Link8</link_name>
    </plugin>
  </gazebo>
-  <gazebo reference="Link6">
-    <sensor name="sensor_contact" type="contact">
+  <gazebo reference="left_contact_panel">
+    <sensor name="left_sensor_contact" type="contact">
      <contact>
-        <collision>Link6_collision</collision>
+        <collision>left_contact_panel_collision</collision>
+      </contact>
+    </sensor>
+  </gazebo>
+  <gazebo reference="right_contact_panel">
+    <sensor name="right_sensor_contact" type="contact">
+      <contact>
+        <collision>right_contact_panel_collision</collision>
      </contact>
    </sensor>
  </gazebo>
--- a/rbs_mill_assist/urdf/rbs_arm.xacro
+++ b/rbs_mill_assist/urdf/rbs_arm.xacro
@ -6,7 +6,7 @@
  <xacro:arg name="tf_prefix" default="" />
  <xacro:arg name="namespace" default="arm0"/>

-  <xacro:arg name="x" default="-0.10" />
+  <xacro:arg name="x" default="-0.20" />
  <xacro:arg name="y" default="0.0" />
  <xacro:arg name="z" default="0.0" />

--- a/rbs_mill_assist/urdf/rbs_arm_macro.xacro
+++ b/rbs_mill_assist/urdf/rbs_arm_macro.xacro
@ -342,45 +342,45 @@
        velocity="10" />
    </joint>
    <link
-      name="Link6">
-      <inertial>
-        <origin
-          xyz="2.3821E-11 -0.054292 0.018171"
-          rpy="0 0 0" />
-        <mass
-          value="0.41222" />
-        <inertia
-          ixx="0.00057892"
-          ixy="-5.0591E-13"
-          ixz="1.1462E-13"
-          iyy="8.7903E-05"
-          iyz="5.3383E-05"
-          izz="0.00063814" />
-      </inertial>
-      <visual>
-        <origin
-          xyz="0 0 0"
-          rpy="0 0 0" />
-        <geometry>
-          <mesh
-            filename="file://$(find rbs_mill_assist)/meshes/Link6.STL" />
-        </geometry>
-        <material
-          name="">
-          <color
-            rgba="0.75294 0.75294 0.75294 1" />
-        </material>
-      </visual>
-      <collision>
-        <origin
-          xyz="0 0 0"
-          rpy="0 0 0" />
-        <geometry>
-          <mesh
-            filename="file://$(find rbs_mill_assist)/meshes/Link6.STL" />
-        </geometry>
-      </collision>
-    </link>
+    name="Link6">
+    <inertial>
+      <origin
+        xyz="1.11237115621161E-06 0.105319882001022 0.0151262038950002"
+        rpy="0 0 0" />
+      <mass
+        value="0.280654829111196" />
+      <inertia
+        ixx="0.00166005184865464"
+        ixy="-2.80959481690827E-08"
+        ixz="-7.84445697318557E-10"
+        iyy="2.87495064989592E-05"
+        iyz="-7.18334236975057E-05"
+        izz="0.00166929050771312" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="file://$(find rbs_mill_assist)/meshes/Link6.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="file://$(find rbs_mill_assist)/meshes/Link6.STL" />
+      </geometry>
+    </collision>
+  </link>
    <joint
      name="ax6"
      type="revolute">
@ -403,16 +403,16 @@
      name="Link7">
      <inertial>
        <origin
-          xyz="3.46944695195361E-18 -5.55111512312578E-17 -0.00360613836994461"
+          xyz="8.67361737988404E-18 0 -0.0036061383699445"
          rpy="0 0 0" />
        <mass
          value="0.0198935071603267" />
        <inertia
          ixx="1.01905343575873E-06"
-          ixy="3.76158192263132E-37"
-          ixz="-3.1449831265618E-23"
+          ixy="1.40555551021685E-36"
+          ixz="-7.39766847562968E-22"
          iyy="1.01905343575873E-06"
-          iyz="6.29843212132371E-23"
+          iyz="-2.63361925044231E-22"
          izz="1.43041044988815E-06" />
      </inertial>
      <visual>
@ -443,7 +443,7 @@
      name="Right_point"
      type="fixed">
      <origin
-        xyz="0.015 -0.095 0.05"
+        xyz="0.015 0.195 0.05"
        rpy="0 0 0" />
      <parent
        link="Link6" />
@ -453,64 +453,64 @@
        xyz="0 0 0" />
    </joint>
    <link
-      name="Link8">
-      <inertial>
-        <origin
-          xyz="0 -2.77555756156289E-17 -0.00360613836994461"
-          rpy="0 0 0" />
-        <mass
-          value="0.0198935071603267" />
-        <inertia
-          ixx="1.01905343575873E-06"
-          ixy="0"
-          ixz="-2.89418945570099E-23"
-          iyy="1.01905343575872E-06"
-          iyz="-1.78775100697742E-23"
-          izz="1.43041044988815E-06" />
-      </inertial>
-      <visual>
-        <origin
-          xyz="0 0 0"
-          rpy="0 0 0" />
-        <geometry>
-          <mesh
-            filename="file://$(find rbs_mill_assist)/meshes/Link8.STL" />
-        </geometry>
-        <material
-          name="">
-          <color
-            rgba="1 1 1 0.4" />
-        </material>
-      </visual>
-      <collision>
-        <origin
-          xyz="0 0 0"
-          rpy="0 0 0" />
-        <geometry>
-          <mesh
-            filename="file://$(find rbs_mill_assist)/meshes/Link8.STL" />
-        </geometry>
-      </collision>
-    </link>
-    <joint
-      name="Left_point"
-      type="fixed">
+    name="Link8">
+    <inertial>
      <origin
-        xyz="-0.015 -0.095 0.05"
+        xyz="1.04083408558608E-17 0 -0.0036061383699445"
        rpy="0 0 0" />
-      <parent
-        link="Link6" />
-      <child
-        link="Link8" />
-      <axis
-        xyz="0 0 0" />
-    </joint>
-    <link name="grasp_point"/>
+      <mass
+        value="0.0198935071603267" />
+      <inertia
+        ixx="1.01905343575873E-06"
+        ixy="1.10608362843633E-36"
+        ixz="-7.54785908468325E-22"
+        iyy="1.01905343575873E-06"
+        iyz="-2.63361925044231E-22"
+        izz="1.43041044988815E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="file://$(find rbs_mill_assist)/meshes/Link8.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 0.4" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="file://$(find rbs_mill_assist)/meshes/Link8.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="Left_point"
+    type="fixed">
+    <origin
+      xyz="-0.015 0.195 0.05"
+      rpy="0 0 0" />
+    <parent
+      link="Link6" />
+    <child
+      link="Link8" />
+    <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"
+        xyz="0.0 0.195 0.05"
        rpy="0 0 0" />
      <parent
        link="Link6" />
@ -520,14 +520,95 @@
        xyz="0 0 0" />
    </joint>

+    <xacro:property name="contact_panel_mass" value="0.0198935071603267" />
+    <xacro:property name="box_x" value="0.01" />
+    <xacro:property name="box_y" value="0.01" />
+    <xacro:property name="box_z" value="0.001" />
+    <xacro:property name="contact_panel_ixx"
+      value="${(1/12.0) * contact_panel_mass * (box_y*box_y + box_z*box_z)}" />
+    <xacro:property name="contact_panel_iyy"
+      value="${(1/12.0) * contact_panel_mass * (box_x*box_x + box_z*box_z)}" />
+    <xacro:property name="contact_panel_izz"
+      value="${(1/12.0) * contact_panel_mass * (box_x*box_x + box_y*box_y)}" />
+
+    <link name="right_contact_panel">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <mass value="${contact_panel_mass}" />
+        <inertia
+          ixx="${contact_panel_ixx}"
+          ixy="0"
+          ixz="0"
+          iyy="${contact_panel_iyy}"
+          iyz="0"
+          izz="${contact_panel_izz}" />
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <box size="${box_x} ${box_y} ${box_z}" />
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 0.4" />
+        </material>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <box size="${box_x} ${box_y} ${box_z}" />
+        </geometry>
+      </collision>
+    </link>
+    <link name="left_contact_panel">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <mass value="${contact_panel_mass}" />
+        <inertia
+          ixx="${contact_panel_ixx}"
+          ixy="0"
+          ixz="0"
+          iyy="${contact_panel_iyy}"
+          iyz="0"
+          izz="${contact_panel_izz}" />
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <box size="${box_x} ${box_y} ${box_z}" />
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 0.4" />
+        </material>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <box size="${box_x} ${box_y} ${box_z}" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="right_contact_panel_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 0" />
+      <parent link="Link8" />
+      <child link="right_contact_panel" />
+    </joint>
+    <joint name="left_contact_panel_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 0" />
+      <parent link="Link7" />
+      <child link="left_contact_panel" />
+    </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:gazebo_reference joint_name="Right_point" />
+    <xacro:gazebo_reference joint_name="Left_point" />
+    <xacro:gazebo_reference joint_name="right_contact_panel_joint" />
+    <xacro:gazebo_reference joint_name="left_contact_panel_joint" />


    <xacro:joint_hardware joint_name="ax1" hardware="${hardware}" initial_joint_position="0.0" />
@ -535,15 +616,15 @@
    <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="1.0" />
-    <xacro:joint_hardware joint_name="ax6" hardware="${hardware}" initial_joint_position="3.14159" />
+    <xacro:joint_hardware joint_name="ax6" hardware="${hardware}" initial_joint_position="0.0" />


    <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>
+      <!-- <xacro:rgbd parent="world" 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>
@ -556,17 +637,17 @@
        </plugin>
      </gazebo>
    </xacro:if>
-    <gazebo reference="Link8">
+    <gazebo reference="left_contact_panel">
      <sensor name='left_sensor_contact' type='contact'>
        <contact>
-          <collision>Link8_collision</collision>
+          <collision>left_contact_panel_collision</collision>
        </contact>
      </sensor>
    </gazebo>
-    <gazebo reference="Link7">
+    <gazebo reference="right_contact_panel">
      <sensor name='right_sensor_contact' type='contact'>
        <contact>
-          <collision>Link7_collision</collision>
+          <collision>right_contact_panel_collision</collision>
        </contact>
      </sensor>
    </gazebo>
--- a/rbs_mill_assist/world/default.sdf
+++ b/rbs_mill_assist/world/default.sdf
@ -37,7 +37,7 @@
      name="gz::sim::systems::SceneBroadcaster">
    </plugin>
    <plugin filename="gz-sim-contact-system"
-            name="gz::sim::systems::Contact">
+      name="gz::sim::systems::Contact">
    </plugin>

    <light type="directional" name="sun">
@ -53,71 +53,108 @@
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
    </light>
-    
+
    <include>
      <name>shildik_0</name>
-      <pose>0.0 -0.28 0.0 0 0 3.14159</pose>
+      <pose>-0.5 -0.2 0.02 0 0 1.57</pose>
      <uri>model://shildik</uri>
    </include>
    <include>
      <name>shildik_1</name>
-      <pose>0.0 -0.28 0.01 0 0 3.14159</pose>
+      <pose>-0.5 -0.2 0.04 0 0 1.57</pose>
      <uri>model://shildik</uri>
    </include>
    <include>
      <name>shildik_2</name>
-      <pose>0.0 -0.28 0.02 0 0 3.14159</pose>
+      <pose>-0.5 -0.2 0.05 0 0 1.57</pose>
      <uri>model://shildik</uri>
    </include>
    <include>
      <name>shildik_3</name>
-      <pose>0.0 -0.28 0.03 0 0 3.14159</pose>
+      <pose>-0.5 -0.2 0.06 0 0 1.57</pose>
      <uri>model://shildik</uri>
    </include>
    <include>
      <name>shildik_4</name>
-      <pose>0.0 -0.28 0.04 0 0 3.14159</pose>
+      <pose>-0.5 -0.2 0.07 0 0 1.57</pose>
      <uri>model://shildik</uri>
    </include>

    <include>
      <static>true</static>
-      <pose>0.30 0.0 0 0 0 -1.57</pose>
+      <pose>0.30 0.0 0 0 0 3.14159</pose>
      <uri>model://laser</uri>
    </include>

-    <!-- <include> -->
-    <!--   <static>true</static> -->
-    <!--   <pose>0.0 -0.3 0.0752 1.57 0 3.14159</pose> -->
-    <!--   <uri>model://bunker</uri> -->
-    <!-- </include> -->
+    <include>
+      <static>true</static>
+      <pose>-0.505 -0.20 0.06 0.0 0.0 1.57</pose>
+      <uri>model://bunker</uri>
+    </include>
+
+    <include>
+      <static>true</static>
+      <pose>0.20 0 0.01 0 0 1.57</pose>
+      <uri>model://conductor</uri>
+    </include>
+
+    <model name="rgbd_camera">
+      <pose>0 -0.93 0.42 0 0.4 1.81</pose>
+      <static>1</static>
+      <link name="rgbd_camera">
+
+        <!-- Visual -->
+        <visual name="visual">
+          <pose>0 0 0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.01 0.01 0.01</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>0 0 0 1</ambient>
+            <diffuse>0 0 0 1</diffuse>
+            <specular>0 0 0 1</specular>
+          </material>
+        </visual>
+
+        <!-- Collision -->
+        <collision name="collision">
+          <pose>0 0 0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.01 0.01 0.01</size>
+            </box>
+          </geometry>
+        </collision>
+
+        <!-- RGBD Sensor -->
+        <sensor name="rgbd_camera" type="rgbd_camera">
+          <camera>
+            <horizontal_fov>1.047</horizontal_fov>
+            <image>
+              <width>1920</width>
+              <height>1080</height>
+              <format>R8G8B8</format>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>3</far>
+            </clip>
+            <depth_camera>
+              <output>depth</output>
+            </depth_camera>
+          </camera>
+          <always_on>1</always_on>
+          <update_rate>30</update_rate>
+          <visualize>true</visualize>
+          <topic>/rgbd_camera/image</topic>
+          <enable_metrics>true</enable_metrics>
+        </sensor>
+      </link>
+    </model>
+

-    <!-- <model name="ground_plane"> -->
-    <!--   <static>true</static> -->
-    <!--   <link name="link"> -->
-    <!--     <collision name="collision"> -->
-    <!--       <geometry> -->
-    <!--         <plane> -->
-    <!--           <normal>0 0 1</normal> -->
-    <!--           <size>1 1</size> -->
-    <!--         </plane> -->
-    <!--       </geometry> -->
-    <!--     </collision> -->
-    <!--     <visual name="visual"> -->
-    <!--       <geometry> -->
-    <!--         <plane> -->
-    <!--           <normal>0 0 1</normal> -->
-    <!--           <size>1 1</size> -->
-    <!--         </plane> -->
-    <!--       </geometry> -->
-    <!--       <material> -->
-    <!--         <ambient>0.8 0.8 0.8 1</ambient> -->
-    <!--         <diffuse>0.8 0.8 0.8 1</diffuse> -->
-    <!--         <specular>0.8 0.8 0.8 1</specular> -->
-    <!--       </material> -->
-    <!--     </visual> -->
-    <!--   </link> -->
-    <!-- </model> -->
    <model name="table">
      <static>1</static>
      <link name='table_link'>
@ -143,10 +180,20 @@
          </geometry>
          <surface>
            <friction>
-              <ode/>
+              <ode>
+                <mu>1.0</mu>
+                <mu2>1.0</mu2>
+              </ode>
            </friction>
-            <bounce/>
-            <contact/>
+            <bounce />
+            <contact>
+              <ode>
+                <soft_cfm>0.00001</soft_cfm>
+                <soft_erp>0.8</soft_erp>
+                <kp>1e5</kp>
+                <kd>1e3</kd>
+              </ode>
+            </contact>
          </surface>
        </collision>
        <visual name='table_link_visual'>
@ -156,10 +203,12 @@
              <size>1.2 0.69999999999999996 0.80000000000000004</size>
            </box>
          </geometry>
-          <material>
-            <diffuse>0 0 1 1</diffuse>
-            <ambient>0 0 1 1</ambient>
-          </material>
+        <material>
+          <diffuse>0.6 0.4 0.2 1</diffuse>
+          <ambient>0.6 0.4 0.2 1</ambient>
+          <specular>0.1 0.1 0.1 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
        </visual>
        <enable_wind>false</enable_wind>
      </link>
--- a/solidworks_files/Label_bunker/Bunker_50mm_height_for_labels_60х40.SLDPRT
+++ b/solidworks_files/Label_bunker/Bunker_50mm_height_for_labels_60х40.SLDPRT
--- a/solidworks_files/Label_bunker/Bunker_50mm_height_for_labels_60х40.STL
+++ b/solidworks_files/Label_bunker/Bunker_50mm_height_for_labels_60х40.STL
--- a/solidworks_files/Label_bunker/Bunker_50mm_height_for_labels_60х40_4.STL
+++ b/solidworks_files/Label_bunker/Bunker_50mm_height_for_labels_60х40_4.STL
--- a/solidworks_files/Label_bunker/Vertical_gripper.STL
+++ b/solidworks_files/Label_bunker/Vertical_gripper.STL
--- a/solidworks_files/RTC_scene/Camera.SLDPRT
+++ b/solidworks_files/RTC_scene/Camera.SLDPRT
--- a/solidworks_files/RTC_scene/asm_scene.SLDASM
+++ b/solidworks_files/RTC_scene/asm_scene.SLDASM
--- a/solidworks_files/RTC_scene/Основание.SLDPRT
+++ b/solidworks_files/RTC_scene/Основание.SLDPRT
--- a/solidworks_files/RTC_scene/Уголок_1200мм.SLDPRT
+++ b/solidworks_files/RTC_scene/Уголок_1200мм.SLDPRT
--- a/solidworks_files/RTC_scene/Уголок_700мм.SLDPRT
+++ b/solidworks_files/RTC_scene/Уголок_700мм.SLDPRT
--- a/solidworks_files/URDF/assembly_6DoF.zip
+++ b/solidworks_files/URDF/assembly_6DoF.zip
--- a/solidworks_files/URDF/assembly_6DoF/CMakeLists.txt
+++ b/solidworks_files/URDF/assembly_6DoF/CMakeLists.txt
@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+project(assembly_6DoF)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+find_package(roslaunch)
+
+foreach(dir config launch meshes urdf)
+	install(DIRECTORY ${dir}/
+		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
+endforeach(dir)
--- a/solidworks_files/URDF/assembly_6DoF/config/joint_names_assembly_6DoF.yaml
+++ b/solidworks_files/URDF/assembly_6DoF/config/joint_names_assembly_6DoF.yaml
@ -0,0 +1 @@
+controller_joint_names: ['', 'ax_1', 'ax_2', 'ax_3', 'ax_4', 'ax_5', 'ax_6', ]
--- a/solidworks_files/URDF/assembly_6DoF/export.log
+++ b/solidworks_files/URDF/assembly_6DoF/export.log
--- a/solidworks_files/URDF/assembly_6DoF/launch/display.launch
+++ b/solidworks_files/URDF/assembly_6DoF/launch/display.launch
@ -0,0 +1,20 @@
+<launch>
+  <arg
+    name="model" />
+  <param
+    name="robot_description"
+    textfile="$(find assembly_6DoF)/urdf/assembly_6DoF.urdf" />
+  <node
+    name="joint_state_publisher_gui"
+    pkg="joint_state_publisher_gui"
+    type="joint_state_publisher_gui" />
+  <node
+    name="robot_state_publisher"
+    pkg="robot_state_publisher"
+    type="robot_state_publisher" />
+  <node
+    name="rviz"
+    pkg="rviz"
+    type="rviz"
+    args="-d $(find assembly_6DoF)/urdf.rviz" />
+</launch>
--- a/solidworks_files/URDF/assembly_6DoF/launch/gazebo.launch
+++ b/solidworks_files/URDF/assembly_6DoF/launch/gazebo.launch
@ -0,0 +1,20 @@
+<launch>
+  <include
+    file="$(find gazebo_ros)/launch/empty_world.launch" />
+  <node
+    name="tf_footprint_base"
+    pkg="tf"
+    type="static_transform_publisher"
+    args="0 0 0 0 0 0 base_link base_footprint 40" />
+  <node
+    name="spawn_model"
+    pkg="gazebo_ros"
+    type="spawn_model"
+    args="-file $(find assembly_6DoF)/urdf/assembly_6DoF.urdf -urdf -model assembly_6DoF"
+    output="screen" />
+  <node
+    name="fake_joint_calibration"
+    pkg="rostopic"
+    type="rostopic"
+    args="pub /calibrated std_msgs/Bool true" />
+</launch>
--- a/solidworks_files/URDF/assembly_6DoF/meshes/Link_02.STL
+++ b/solidworks_files/URDF/assembly_6DoF/meshes/Link_02.STL
--- a/solidworks_files/URDF/assembly_6DoF/meshes/Link_03.STL
+++ b/solidworks_files/URDF/assembly_6DoF/meshes/Link_03.STL
--- a/solidworks_files/URDF/assembly_6DoF/meshes/Link_04.STL
+++ b/solidworks_files/URDF/assembly_6DoF/meshes/Link_04.STL
--- a/solidworks_files/URDF/assembly_6DoF/meshes/Link_06.STL
+++ b/solidworks_files/URDF/assembly_6DoF/meshes/Link_06.STL
--- a/solidworks_files/URDF/assembly_6DoF/meshes/Link_07.STL
+++ b/solidworks_files/URDF/assembly_6DoF/meshes/Link_07.STL
--- a/solidworks_files/URDF/assembly_6DoF/meshes/Link_08.STL
+++ b/solidworks_files/URDF/assembly_6DoF/meshes/Link_08.STL
--- a/solidworks_files/URDF/assembly_6DoF/meshes/base_link.STL
+++ b/solidworks_files/URDF/assembly_6DoF/meshes/base_link.STL
--- a/solidworks_files/URDF/assembly_6DoF/meshes/link_01.STL
+++ b/solidworks_files/URDF/assembly_6DoF/meshes/link_01.STL
--- a/solidworks_files/URDF/assembly_6DoF/package.xml
+++ b/solidworks_files/URDF/assembly_6DoF/package.xml
@ -0,0 +1,21 @@
+<package format="2">
+  <name>assembly_6DoF</name>
+  <version>1.0.0</version>
+  <description>
+    <p>URDF Description package for assembly_6DoF</p>
+    <p>This package contains configuration data, 3D models and launch files
+for assembly_6DoF robot</p>
+  </description>
+  <author>TODO</author>
+  <maintainer email="TODO@email.com" />
+  <license>BSD</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>roslaunch</depend>
+  <depend>robot_state_publisher</depend>
+  <depend>rviz</depend>
+  <depend>joint_state_publisher_gui</depend>
+  <depend>gazebo</depend>
+  <export>
+    <architecture_independent />
+  </export>
+</package>
--- a/solidworks_files/URDF/assembly_6DoF/urdf/assembly_6DoF.csv
+++ b/solidworks_files/URDF/assembly_6DoF/urdf/assembly_6DoF.csv
@ -0,0 +1,10 @@
+Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
+base_link,"-8,54233606739125E-05","0,00237261516674613","0,0727412122189275",0,0,0,"1,22750289216444","0,00132489346534405","-9,04885171217137E-06","9,12449155936019E-06","0,00163265405627164","5,56490919358566E-05","0,00208032106706038",0,0,0,0,0,0,package://assembly_6DoF/meshes/base_link.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://assembly_6DoF/meshes/base_link.STL,,ASSEMBLY_START-1,Origin_global,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
+link_01,"-0,00848908618604632","-0,00280906081849381","0,0914408492450371",0,0,0,"0,515224814650532","0,00161165592122362","5,3037835070687E-07","-3,15413458468469E-05","0,00190493381759602","5,09548827879937E-06","0,000857659568521175",0,0,0,0,0,0,package://assembly_6DoF/meshes/link_01.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://assembly_6DoF/meshes/link_01.STL,,ASSEMBLY_FORK_L-2,Origin_ax_1,Axis_ax_1,ax_1,revolute,0,0,"0,12651",0,0,0,base_link,0,0,1,0,0,0,0,,,,,,,,
+Link_02,"-4,10661897513285E-07","0,00223496402317745","0,0459918009610263",0,0,0,"1,41722285650076","0,00186290156875659","-3,00830959226468E-09","4,43975425989195E-05","0,001769032367939","-3,03156147105806E-09","0,000726634720349098",0,0,0,0,0,0,package://assembly_6DoF/meshes/Link_02.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://assembly_6DoF/meshes/Link_02.STL,,ASSEMBLY_FIRST-1,Origin_ax_2,Axis_ax_2,ax_2,revolute,0,0,"0,15929",0,0,0,link_01,1,0,0,0,0,0,0,,,,,,,,
+Link_03,"0,00951548546402769","-0,00124545789767392","0,0426605611304918",0,0,0,"0,42702159219928","0,000803989256561299","-3,01612394083865E-06","-1,61263808941143E-05","0,00100796753887135","-2,64241274321953E-06","0,000637842610902841",0,0,0,0,0,0,package://assembly_6DoF/meshes/Link_03.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://assembly_6DoF/meshes/Link_03.STL,,ASSEMBLY_FORK_S-1,Origin_ax_3,Axis_ax_3,ax_3,revolute,0,0,"0,092",0,0,0,Link_02,-1,0,0,0,0,0,0,,,,,,,,
+Link_04,"-0,000334339865101156","0,00464836357845911","0,0651901109141136",0,0,0,"0,859217743981029","0,000582593656622538","6,56596271695798E-06","1,04038014699374E-05","0,000594739623571533","-2,05954986474142E-06","0,000325704958954311",0,0,0,0,0,0,package://assembly_6DoF/meshes/Link_04.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://assembly_6DoF/meshes/Link_04.STL,,ASSEMBLY_SECOND_P1-1,Origin_ax_4,Axis_ax_4,ax_4,revolute,0,0,"0,094901",0,0,0,Link_03,0,0,-1,0,0,0,0,,,,,,,,
+Link_05,"0,000453829432802026","0,000829845158224828","0,071897039942263",0,0,0,"0,695050888352266","0,000507055068179218","3,50327671939265E-07","4,50703805386992E-06","0,000775145776409889","-6,3960847270042E-06","0,000756243961469555",0,0,0,0,0,0,package://assembly_6DoF/meshes/Link_05.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://assembly_6DoF/meshes/Link_05.STL,,ASSEMBLY_SECOND_P2-1,Origin_ax_5,Axis_ax_5,ax_5,revolute,0,0,"0,097999",0,0,0,Link_04,-1,0,0,0,0,0,0,,,,,,,,
+Link_06,"-8,8831466371099E-09","-0,112071334546287","0,00810241687772562",0,0,0,"0,280330762522721","0,00129459678338959","-9,83110094591847E-11","-3,19830442893476E-11","2,09759800769375E-05","5,63414300510928E-05","0,00129498421400291",0,0,0,0,0,0,package://assembly_6DoF/meshes/Link_06.STL,"0,752941176470588","0,752941176470588","0,752941176470588",1,0,0,0,0,0,0,package://assembly_6DoF/meshes/Link_06.STL,,asm_vacuum_gripper_vertical-1,Origin_ax_6,Axis_ax_6,ax_6,revolute,"-0,0012",0,"0,1145",0,0,0,Link_05,0,0,1,0,0,0,0,,,,,,,,
+Link_07,"-1,01394587170844E-15","-5,55111512312578E-17","-0,0049950359465929",0,0,0,"0,0119295697834755","4,27745590342647E-07","2,68928440312099E-33","1,44687693702654E-21","4,2774559034265E-07","3,0329437472821E-21","3,81314853129931E-07",0,0,0,0,0,0,package://assembly_6DoF/meshes/Link_07.STL,1,1,1,"0,4",0,0,0,0,0,0,package://assembly_6DoF/meshes/Link_07.STL,,asm_vacuum_gripper_vertical-1/Vacuum_lifter(cup)-2,Origin_ax_7,,ax_7,fixed,0,"-0,19","0,04",0,0,0,Link_06,0,0,0,,,,,,,,,,,,
+Link_08,"-6,10622663543836E-16","-1,11022302462516E-16","-0,0049950359465929",0,0,0,"0,0119295697834755","4,2774559034265E-07","-2,75001183405761E-33","3,1360121197781E-21","4,27745590342647E-07","-5,45683760877208E-22","3,81314853129931E-07",0,0,0,0,0,0,package://assembly_6DoF/meshes/Link_08.STL,1,1,1,"0,4",0,0,0,0,0,0,package://assembly_6DoF/meshes/Link_08.STL,,asm_vacuum_gripper_vertical-1/Vacuum_lifter(cup)-1,Origin_ax_8,Axis_ax_8,ax_8,fixed,0,"-0,164999999999954","0,0399999999999993",0,0,0,Link_06,0,0,0,,,,,,,,,,,,
--- a/solidworks_files/URDF/assembly_6DoF/urdf/assembly_6DoF.urdf
+++ b/solidworks_files/URDF/assembly_6DoF/urdf/assembly_6DoF.urdf
@ -0,0 +1,501 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
+     Commit Version: 1.6.0-4-g7f85cfe  Build Version: 1.6.7995.38578
+     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
+<robot
+  name="assembly_6DoF">
+  <link
+    name="base_link">
+    <inertial>
+      <origin
+        xyz="-8.54233606739125E-05 0.00237261516674613 0.0727412122189275"
+        rpy="0 0 0" />
+      <mass
+        value="1.22750289216444" />
+      <inertia
+        ixx="0.00132489346534405"
+        ixy="-9.04885171217137E-06"
+        ixz="9.12449155936019E-06"
+        iyy="0.00163265405627164"
+        iyz="5.56490919358566E-05"
+        izz="0.00208032106706038" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://assembly_6DoF/meshes/base_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://assembly_6DoF/meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link
+    name="link_01">
+    <inertial>
+      <origin
+        xyz="-0.00848908618604632 -0.00280906081849381 0.0914408492450371"
+        rpy="0 0 0" />
+      <mass
+        value="0.515224814650532" />
+      <inertia
+        ixx="0.00161165592122362"
+        ixy="5.3037835070687E-07"
+        ixz="-3.15413458468469E-05"
+        iyy="0.00190493381759602"
+        iyz="5.09548827879937E-06"
+        izz="0.000857659568521175" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://assembly_6DoF/meshes/link_01.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://assembly_6DoF/meshes/link_01.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="ax_1"
+    type="revolute">
+    <origin
+      xyz="0 0 0.12651"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="link_01" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="0"
+      upper="0"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="Link_02">
+    <inertial>
+      <origin
+        xyz="-4.10661897513285E-07 0.00223496402317745 0.0459918009610263"
+        rpy="0 0 0" />
+      <mass
+        value="1.41722285650076" />
+      <inertia
+        ixx="0.00186290156875659"
+        ixy="-3.00830959226468E-09"
+        ixz="4.43975425989195E-05"
+        iyy="0.001769032367939"
+        iyz="-3.03156147105806E-09"
+        izz="0.000726634720349098" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://assembly_6DoF/meshes/Link_02.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://assembly_6DoF/meshes/Link_02.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="ax_2"
+    type="revolute">
+    <origin
+      xyz="0 0 0.15929"
+      rpy="0 0 0" />
+    <parent
+      link="link_01" />
+    <child
+      link="Link_02" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="0"
+      upper="0"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="Link_03">
+    <inertial>
+      <origin
+        xyz="0.00951548546402769 -0.00124545789767392 0.0426605611304918"
+        rpy="0 0 0" />
+      <mass
+        value="0.42702159219928" />
+      <inertia
+        ixx="0.000803989256561299"
+        ixy="-3.01612394083865E-06"
+        ixz="-1.61263808941143E-05"
+        iyy="0.00100796753887135"
+        iyz="-2.64241274321953E-06"
+        izz="0.000637842610902841" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://assembly_6DoF/meshes/Link_03.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://assembly_6DoF/meshes/Link_03.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="ax_3"
+    type="revolute">
+    <origin
+      xyz="0 0 0.092"
+      rpy="0 0 0" />
+    <parent
+      link="Link_02" />
+    <child
+      link="Link_03" />
+    <axis
+      xyz="-1 0 0" />
+    <limit
+      lower="0"
+      upper="0"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="Link_04">
+    <inertial>
+      <origin
+        xyz="-0.000334339865101156 0.00464836357845911 0.0651901109141136"
+        rpy="0 0 0" />
+      <mass
+        value="0.859217743981029" />
+      <inertia
+        ixx="0.000582593656622538"
+        ixy="6.56596271695798E-06"
+        ixz="1.04038014699374E-05"
+        iyy="0.000594739623571533"
+        iyz="-2.05954986474142E-06"
+        izz="0.000325704958954311" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://assembly_6DoF/meshes/Link_04.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://assembly_6DoF/meshes/Link_04.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="ax_4"
+    type="revolute">
+    <origin
+      xyz="0 0 0.094901"
+      rpy="0 0 0" />
+    <parent
+      link="Link_03" />
+    <child
+      link="Link_04" />
+    <axis
+      xyz="0 0 -1" />
+    <limit
+      lower="0"
+      upper="0"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="Link_05">
+    <inertial>
+      <origin
+        xyz="0.000453829432802026 0.000829845158224828 0.071897039942263"
+        rpy="0 0 0" />
+      <mass
+        value="0.695050888352266" />
+      <inertia
+        ixx="0.000507055068179218"
+        ixy="3.50327671939265E-07"
+        ixz="4.50703805386992E-06"
+        iyy="0.000775145776409889"
+        iyz="-6.3960847270042E-06"
+        izz="0.000756243961469555" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://assembly_6DoF/meshes/Link_05.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://assembly_6DoF/meshes/Link_05.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="ax_5"
+    type="revolute">
+    <origin
+      xyz="0 0 0.097999"
+      rpy="0 0 0" />
+    <parent
+      link="Link_04" />
+    <child
+      link="Link_05" />
+    <axis
+      xyz="-1 0 0" />
+    <limit
+      lower="0"
+      upper="0"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="Link_06">
+    <inertial>
+      <origin
+        xyz="-8.8831466371099E-09 -0.112071334546287 0.00810241687772562"
+        rpy="0 0 0" />
+      <mass
+        value="0.280330762522721" />
+      <inertia
+        ixx="0.00129459678338959"
+        ixy="-9.83110094591847E-11"
+        ixz="-3.19830442893476E-11"
+        iyy="2.09759800769375E-05"
+        iyz="5.63414300510928E-05"
+        izz="0.00129498421400291" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://assembly_6DoF/meshes/Link_06.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://assembly_6DoF/meshes/Link_06.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="ax_6"
+    type="revolute">
+    <origin
+      xyz="-0.0012 0 0.1145"
+      rpy="0 0 0" />
+    <parent
+      link="Link_05" />
+    <child
+      link="Link_06" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="0"
+      upper="0"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="Link_07">
+    <inertial>
+      <origin
+        xyz="-1.01394587170844E-15 -5.55111512312578E-17 -0.0049950359465929"
+        rpy="0 0 0" />
+      <mass
+        value="0.0119295697834755" />
+      <inertia
+        ixx="4.27745590342647E-07"
+        ixy="2.68928440312099E-33"
+        ixz="1.44687693702654E-21"
+        iyy="4.2774559034265E-07"
+        iyz="3.0329437472821E-21"
+        izz="3.81314853129931E-07" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://assembly_6DoF/meshes/Link_07.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 0.4" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://assembly_6DoF/meshes/Link_07.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="ax_7"
+    type="fixed">
+    <origin
+      xyz="0 -0.19 0.04"
+      rpy="0 0 0" />
+    <parent
+      link="Link_06" />
+    <child
+      link="Link_07" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="Link_08">
+    <inertial>
+      <origin
+        xyz="-6.10622663543836E-16 -1.11022302462516E-16 -0.0049950359465929"
+        rpy="0 0 0" />
+      <mass
+        value="0.0119295697834755" />
+      <inertia
+        ixx="4.2774559034265E-07"
+        ixy="-2.75001183405761E-33"
+        ixz="3.1360121197781E-21"
+        iyy="4.27745590342647E-07"
+        iyz="-5.45683760877208E-22"
+        izz="3.81314853129931E-07" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://assembly_6DoF/meshes/Link_08.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 0.4" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://assembly_6DoF/meshes/Link_08.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="ax_8"
+    type="fixed">
+    <origin
+      xyz="0 -0.164999999999954 0.0399999999999993"
+      rpy="0 0 0" />
+    <parent
+      link="Link_06" />
+    <child
+      link="Link_08" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+</robot>
--- a/solidworks_files/URDF/assembly_6DoF_with_gripper.zip
+++ b/solidworks_files/URDF/assembly_6DoF_with_gripper.zip
--- a/solidworks_files/URDF/assembly_6DoF_with_gripper/CMakeLists.txt
+++ b/solidworks_files/URDF/assembly_6DoF_with_gripper/CMakeLists.txt
@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+project(assembly_6DoF_with_gripper)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+find_package(roslaunch)
+
+foreach(dir config launch meshes urdf)
+	install(DIRECTORY ${dir}/
+		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
+endforeach(dir)
--- a/solidworks_files/URDF/assembly_6DoF_with_gripper/config/joint_names_assembly_6DoF_with_gripper.yaml
+++ b/solidworks_files/URDF/assembly_6DoF_with_gripper/config/joint_names_assembly_6DoF_with_gripper.yaml
@ -0,0 +1 @@
+controller_joint_names: ['', 'ax_1', 'ax_2', 'ax_3', 'ax_4', 'ax_5', 'ax_6', 'ax_7', 'ax_8', ]
--- a/solidworks_files/URDF/assembly_6DoF_with_gripper/export.log
+++ b/solidworks_files/URDF/assembly_6DoF_with_gripper/export.log
--- a/solidworks_files/URDF/assembly_6DoF_with_gripper/launch/display.launch
+++ b/solidworks_files/URDF/assembly_6DoF_with_gripper/launch/display.launch
@ -0,0 +1,20 @@
+<launch>
+  <arg
+    name="model" />
+  <param
+    name="robot_description"
+    textfile="$(find assembly_6DoF_with_gripper)/urdf/assembly_6DoF_with_gripper.urdf" />
+  <node
+    name="joint_state_publisher_gui"
+    pkg="joint_state_publisher_gui"
+    type="joint_state_publisher_gui" />
+  <node
+    name="robot_state_publisher"
+    pkg="robot_state_publisher"
+    type="robot_state_publisher" />
+  <node
+    name="rviz"
+    pkg="rviz"
+    type="rviz"
+    args="-d $(find assembly_6DoF_with_gripper)/urdf.rviz" />
+</launch>
--- a/solidworks_files/URDF/assembly_6DoF_with_gripper/launch/gazebo.launch
+++ b/solidworks_files/URDF/assembly_6DoF_with_gripper/launch/gazebo.launch
@ -0,0 +1,20 @@
+<launch>
+  <include
+    file="$(find gazebo_ros)/launch/empty_world.launch" />
+  <node
+    name="tf_footprint_base"
+    pkg="tf"
+    type="static_transform_publisher"
+    args="0 0 0 0 0 0 base_link base_footprint 40" />
+  <node
+    name="spawn_model"
+    pkg="gazebo_ros"
+    type="spawn_model"
+    args="-file $(find assembly_6DoF_with_gripper)/urdf/assembly_6DoF_with_gripper.urdf -urdf -model assembly_6DoF_with_gripper"
+    output="screen" />
+  <node
+    name="fake_joint_calibration"
+    pkg="rostopic"
+    type="rostopic"
+    args="pub /calibrated std_msgs/Bool true" />
+</launch>
--- a/solidworks_files/URDF/assembly_6DoF_with_gripper/meshes/Link_02.STL
+++ b/solidworks_files/URDF/assembly_6DoF_with_gripper/meshes/Link_02.STL
--- a/solidworks_files/URDF/assembly_6DoF_with_gripper/meshes/Link_03.STL
+++ b/solidworks_files/URDF/assembly_6DoF_with_gripper/meshes/Link_03.STL
--- a/solidworks_files/URDF/assembly_6DoF_with_gripper/meshes/Link_04.STL
+++ b/solidworks_files/URDF/assembly_6DoF_with_gripper/meshes/Link_04.STL
--- a/solidworks_files/URDF/assembly_6DoF_with_gripper/meshes/Link_06.STL
+++ b/solidworks_files/URDF/assembly_6DoF_with_gripper/meshes/Link_06.STL
--- a/solidworks_files/URDF/assembly_6DoF_with_gripper/meshes/Link_07.STL
+++ b/solidworks_files/URDF/assembly_6DoF_with_gripper/meshes/Link_07.STL
--- a/solidworks_files/URDF/assembly_6DoF_with_gripper/meshes/Link_08.STL
+++ b/solidworks_files/URDF/assembly_6DoF_with_gripper/meshes/Link_08.STL
--- a/solidworks_files/URDF/assembly_6DoF_with_gripper/meshes/base_link.STL
+++ b/solidworks_files/URDF/assembly_6DoF_with_gripper/meshes/base_link.STL
--- a/Show more
+++ b/Show more
Author	SHA1	Message	Date
VladimirLatukhin	d7e1c2c56f	Add URDF. Some changes in scene.	2025-03-19 15:23:57 +03:00
Bill Finger	7c5ba539c5	update MainTree	2025-03-17 17:32:38 +03:00
Bill Finger	9b9c706c97	add twist cmd with condition BT plugin and refactor VacuumGripper to publish contact info update grasping poses	2025-03-17 14:20:21 +03:00
Bill Finger	6c5cf0633c	add conductor to scene - change laser mesh to obj - update place position	2025-03-17 14:20:21 +03:00
Bill Finger	1f7dc11289	add planner configs to planning group in moveit config	2025-03-17 14:20:21 +03:00
Bill Finger	ed430a825d	remove rgbd_camera from srdf	2025-03-17 14:20:21 +03:00
Bill Finger	be37f1422b	update rbs_arm joint limits	2025-03-17 14:20:21 +03:00
Bill Finger	1ad460f4a1	update scene - update to new bunker poses - remove condition and add repeat decorator - update grasping positions due to the new bunker model - format gazebo world file xml and update positions of new scene	2025-03-17 14:20:21 +03:00
Bill Finger	255c7b1b18	fix(gz_bridge): camera topic	2025-03-17 14:20:21 +03:00
shalenikol	b0d61ecc3a	camera pose (rgbd)	2025-03-17 14:20:21 +03:00
Bill Finger	9c6338774f	remove chomp planner from pipelines config and use stomp by default	2025-03-17 14:20:21 +03:00
Bill Finger	4555d2b12e	refactor(moveit launch): migrate to moveit_config_builder	2025-03-17 14:20:21 +03:00
Bill Finger	193e2ef3c9	update scene shildik places	2025-03-17 14:20:21 +03:00
Bill Finger	d6b50f423b	add pose publisher node	2025-03-17 14:20:21 +03:00
Bill Finger	4bbe4e5cdb	add additional planning parameters	2025-03-17 14:20:21 +03:00
Bill Finger	af64efc0a1	update launch files	2025-03-17 14:20:21 +03:00
Bill Finger	5883ac354a	update poses	2025-03-17 14:20:21 +03:00
Bill Finger	52b346f6e0	refactor(MainTree): format code and add new test condition	2025-03-17 14:20:21 +03:00
Bill Finger	5f15b417ab	refactor(Grasp): use MoveitMtp	2025-03-17 14:20:21 +03:00
Bill Finger	f211c0fd8e	refactor(ToGraver): add robot_name to SubTree blackboard	2025-03-17 14:20:21 +03:00
Bill Finger	bb2599a006	refactor(logging): print BT action ID	2025-03-17 14:20:21 +03:00
Bill Finger	2a85dfce18	update time intervals and update cmake deps	2025-03-17 14:20:21 +03:00
Bill Finger	873f54574f	add pinocchio to deps	2025-03-17 14:20:21 +03:00
Bill Finger	4ac6d59941	add workspace publisher	2025-03-17 14:20:21 +03:00
Bill Finger	eac8f088d3	add moveit config and update scene	2025-03-17 14:20:21 +03:00
Bill Finger	d1553e2a9e	update config files. poses and gz-bridge	2025-03-17 14:20:21 +03:00
Bill Finger	ffec1285d4	update laser asset	2025-03-17 14:20:21 +03:00
Bill Finger	2156e9a0c9	Update BT for full process with graver - Adding new fixed joint links (`right_contact_panel` and `left_contact_panel`) to Link8 and Link7. - Defining visual representations with semi-transparent boxes for these panels. - Setting up collision models to enable accurate contact detection. - Updating Gazebo references to maintain fixed joint properties in simulations. - Moving sensors from the main links to the new contact panels for better interaction sensing. - Correcting and updating collision references in sensors. - Making minor XML formatting adjustments for consistency.	2025-03-17 14:20:21 +03:00
				`@ -0,0 +1 @@`
				`controller_joint_names: ['', 'ax_1', 'ax_2', 'ax_3', 'ax_4', 'ax_5', 'ax_6', ]`