init tool_offset with the offset of the first tool added

diff --git a/src/modules/tools/toolmanager/ToolManager.cpp b/src/modules/tools/toolmanager/ToolManager.cpp
index 361ea23..412e70a 100644 (file)
--- a/src/modules/tools/toolmanager/ToolManager.cpp
+++ b/src/modules/tools/toolmanager/ToolManager.cpp
@@ -110,6 +110,9 @@ void ToolManager::add_tool(Tool* tool_to_add){
     if(this->tools.size() == 0){
         tool_to_add->enable();
         this->current_tool_name = tool_to_add->get_name();
+        //send new_tool_offsets to robot
+        float *new_tool_offset = tool_to_add->get_offset();
+        THEKERNEL->robot->setToolOffset(new_tool_offset[0], new_tool_offset[1], new_tool_offset[2]);
     } else {
         tool_to_add->disable();
     }