diff --git a/GraspPlanning/ApproachMovementGenerator.cpp b/GraspPlanning/ApproachMovementGenerator.cpp
index 0d85c3551a04637fffc51cf210e07595114ad86c..5d9e362787350ac41c458a7399e32d5defed8faf 100644
--- a/GraspPlanning/ApproachMovementGenerator.cpp
+++ b/GraspPlanning/ApproachMovementGenerator.cpp
@@ -3,6 +3,7 @@
 #include <VirtualRobot/SceneObject.h>
 #include <VirtualRobot/Visualization/TriMeshModel.h>
 #include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
 #include <VirtualRobot/EndEffector/EndEffector.h>
 #include <VirtualRobot/MathTools.h>
 #include <VirtualRobot/RobotConfig.h>
diff --git a/VirtualRobot/EndEffector/EndEffectorActor.cpp b/VirtualRobot/EndEffector/EndEffectorActor.cpp
index 8e8bcecc025337908f1ee9e3bf0fb697af28699a..02499d5a86532f845e4f6ba3becace876046ce34 100644
--- a/VirtualRobot/EndEffector/EndEffectorActor.cpp
+++ b/VirtualRobot/EndEffector/EndEffectorActor.cpp
@@ -12,6 +12,7 @@
 #include "../RobotConfig.h"
 #include "../SceneObjectSet.h"
 #include "../CollisionDetection/CollisionChecker.h"
+#include "../CollisionDetection/CollisionModel.h"
 #include "EndEffector.h"
 #include "../SceneObjectSet.h"
 
diff --git a/VirtualRobot/IK/ConstrainedIK.cpp b/VirtualRobot/IK/ConstrainedIK.cpp
index b0a9c0d5cee64723b68cf5860536b32e1ee5ce50..ccf8d411204629bb0edd9f4bbca88fd9cbe8cd77 100644
--- a/VirtualRobot/IK/ConstrainedIK.cpp
+++ b/VirtualRobot/IK/ConstrainedIK.cpp
@@ -1,5 +1,7 @@
 #include "ConstrainedIK.h"
 
+#include "../RobotFactory.h"
+
 using namespace VirtualRobot;
 
 using std::cout;
diff --git a/VirtualRobot/Import/COLLADA-light/ColladaSimox.cpp b/VirtualRobot/Import/COLLADA-light/ColladaSimox.cpp
index e5733d7a20fa3e471547d8b05b92674d35fea9a7..30a1f143884d77e09a82d079fcd7fe1f0b70f5f1 100644
--- a/VirtualRobot/Import/COLLADA-light/ColladaSimox.cpp
+++ b/VirtualRobot/Import/COLLADA-light/ColladaSimox.cpp
@@ -3,18 +3,17 @@
 #include <VirtualRobot/Nodes/RobotNodePrismatic.h>
 #include <VirtualRobot/Nodes/RobotNodeRevolute.h>
 #include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
+#include <VirtualRobot/RobotFactory.h>
 
 #include "inventor.h"
 #include "ColladaSimox.h"
 
-#include <boost/foreach.hpp>
-
 #include <Eigen/Dense>
 #include <Eigen/Geometry>
 
 using namespace std;
-//using namespace VirtualRobot;
 
 #define DBG_NODE(NAME) (this->name.compare(NAME)==0)
 
@@ -71,7 +70,7 @@ namespace Collada
         std::vector<VirtualRobot::RobotNodePtr> simoxRevoluteJoints;
         std::vector<VirtualRobot::RobotNodePtr> simoxPrismaticJoints;
 
-        BOOST_FOREACH(ColladaRobotNodePtr node, this->robotNodeSet)
+        for (ColladaRobotNodePtr node : this->robotNodeSet)
         {
             simoxRobotNodeSet.push_back(dynamic_pointer_cast<ColladaSimoxRobotNode>(node)->simoxRobotNode);
 
@@ -86,7 +85,7 @@ namespace Collada
             }
 
             vector<string> children;
-            BOOST_FOREACH(ColladaRobotNodePtr child, node->children)
+            for (ColladaRobotNodePtr child : node->children)
             children.push_back(child->name);
 
             childrenMap[dynamic_pointer_cast<ColladaSimoxRobotNode>(node)->simoxRobotNode] = children;
@@ -152,7 +151,7 @@ namespace Collada
         Eigen::Matrix4f preJointTransformation = Eigen::Matrix4f::Identity();
 
 
-        BOOST_FOREACH(pugi::xml_node node, this->preJoint)
+        for (pugi::xml_node node : this->preJoint)
         {
             preJointTransformation = preJointTransformation * getTransform(node, scaleFactor);
         }
@@ -163,7 +162,7 @@ namespace Collada
             cout << jointLimitLow << "," << jointLimitHigh << "," << acceleration << "," << deceleration << "," << velocity << "," << torque << endl;
             cout << this->joint_axis.name() << endl;
             preJointTransformation = Eigen::Matrix4f::Identity();
-            BOOST_FOREACH(pugi::xml_node node, this->preJoint)
+            for (pugi::xml_node node : this->preJoint)
             {
                 cout << getTransform(node) << endl;
                 preJointTransformation = preJointTransformation * getTransform(node, scaleFactor);
@@ -187,7 +186,7 @@ namespace Collada
             pugi::xml_node technique = rigidBodies[0].child("technique_common");
             float mass = boost::lexical_cast<float>(technique.child("mass").child_value());
             Eigen::Matrix4f massFrameTransformation = Eigen::Matrix4f::Identity();
-            BOOST_FOREACH(pugi::xpath_node trafo, technique.select_nodes(".//mass_frame/*"))
+            for (pugi::xpath_node trafo : technique.select_nodes(".//mass_frame/*"))
             {
                 massFrameTransformation = massFrameTransformation * getTransform(trafo.node(), 1000.0f);
             }
@@ -233,11 +232,11 @@ namespace Collada
         }
 
 #ifdef COLLADA_IMPORT_USE_SENSORS
-        BOOST_FOREACH(pugi::xml_node sensor, this->sensors)
+        for (pugi::xml_node sensor : this->sensors)
         {
             string sensorName = sensor.attribute("name").value();
             Eigen::Matrix4f sensorTransformation = Eigen::Matrix4f::Identity();
-            BOOST_FOREACH(pugi::xpath_node trafo, sensor.select_nodes(".//frame_origin/*"))
+            for (pugi::xpath_node trafo : sensor.select_nodes(".//frame_origin/*"))
             {
                 sensorTransformation = sensorTransformation * getTransform(trafo.node(), scaleFactor);
             }
diff --git a/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp b/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp
index 8db9b77606a86666df519b7319626009fdb012bd..4def377be334de8b1ccd578040e7569ea74cbde9 100644
--- a/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp
+++ b/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp
@@ -1,16 +1,15 @@
 #include "SimoxURDFFactory.h"
 
-#include <assert.h>
 #include <filesystem>
 #include <iostream>
 #include <map>
-#include <stack>
 
 #include <urdf_model/model.h>
 #include <urdf_parser/urdf_parser.h>
 
 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
 #include <VirtualRobot/RobotFactory.h>
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
 #include <VirtualRobot/Nodes/RobotNodeFactory.h>
 #include <VirtualRobot/Nodes/RobotNodeFixedFactory.h>
 #include <VirtualRobot/Nodes/RobotNodePrismaticFactory.h>
diff --git a/VirtualRobot/MathTools.h b/VirtualRobot/MathTools.h
index 5a9ba1efe3e06d0bb5d51529faaa00ac4b8afc9a..674994fbe06f1d3842b028c2f58e7519889c3470 100644
--- a/VirtualRobot/MathTools.h
+++ b/VirtualRobot/MathTools.h
@@ -25,7 +25,6 @@
 #include "VirtualRobot.h"
 
 #include <Eigen/Core>
-#include <Eigen/Geometry>
 
 #include <string>
 #include <vector>
diff --git a/VirtualRobot/Nodes/ConditionedLock.h b/VirtualRobot/Nodes/ConditionedLock.h
index e39920f510b33cdeab52b1ec63df6f9535722d26..fada89dcb589023febe96971aa5f740c9c10549b 100755
--- a/VirtualRobot/Nodes/ConditionedLock.h
+++ b/VirtualRobot/Nodes/ConditionedLock.h
@@ -29,8 +29,7 @@ public:
 
 typedef ConditionedLock<std::unique_lock<std::recursive_mutex> > ReadLock;
 typedef ConditionedLock<std::unique_lock<std::recursive_mutex> > WriteLock;
-//typedef ConditionedLock<boost::shared_lock<boost::shared_mutex> > ReadLock;
-//typedef ConditionedLock<boost::unique_lock<boost::shared_mutex> > WriteLock;
+
 typedef std::shared_ptr< ReadLock > ReadLockPtr;
 typedef std::shared_ptr< WriteLock > WriteLockPtr;
 
diff --git a/VirtualRobot/Nodes/RobotNode.cpp b/VirtualRobot/Nodes/RobotNode.cpp
index ddf4d11304064c1c32c371f9ef4a7fc527ae201c..e54c52a2d16d96551e49e0f7d64202690a8a719c 100644
--- a/VirtualRobot/Nodes/RobotNode.cpp
+++ b/VirtualRobot/Nodes/RobotNode.cpp
@@ -4,6 +4,7 @@
 #include "../VirtualRobotException.h"
 #include "../Robot.h"
 #include "../RobotNodeSet.h"
+#include "../CollisionDetection/CollisionModel.h"
 #include "../Visualization/VisualizationFactory.h"
 #include "../Visualization/Visualization.h"
 #include "../Visualization/TriMeshModel.h"
diff --git a/VirtualRobot/Nodes/RobotNode.h b/VirtualRobot/Nodes/RobotNode.h
index 2afa882744ba7d2e168857a931c5096bf3b3e070..5d007274283682513afd1d2e69f56f0101521438 100644
--- a/VirtualRobot/Nodes/RobotNode.h
+++ b/VirtualRobot/Nodes/RobotNode.h
@@ -22,21 +22,14 @@
 */
 #pragma once
 
-#include "../VirtualRobot.h"
-#include "../VirtualRobotException.h"
 #include "../VirtualRobot.h"
 
 #include "../SceneObject.h"
-#include "../RobotFactory.h"
-#include "../CollisionDetection/CollisionModel.h"
 #include "../Transformation/DHParameter.h"
 #include "../Visualization/VisualizationNode.h"
-#include "ConditionedLock.h"
 #include "Sensor.h"
 
 #include <Eigen/Core>
-#include <Eigen/Geometry>
-
 
 #include <string>
 #include <vector>
diff --git a/VirtualRobot/Nodes/RobotNodeFixed.h b/VirtualRobot/Nodes/RobotNodeFixed.h
index 33c336c23f690eb5767e2558dca890254c37a916..1ed7a0dc50c15cf86f563162badc67e8dd6f636c 100644
--- a/VirtualRobot/Nodes/RobotNodeFixed.h
+++ b/VirtualRobot/Nodes/RobotNodeFixed.h
@@ -25,7 +25,6 @@
 #include "../VirtualRobot.h"
 
 #include "RobotNode.h"
-#include "../RobotFactory.h"
 
 #include <Eigen/Core>
 
diff --git a/VirtualRobot/Nodes/RobotNodeFixedFactory.cpp b/VirtualRobot/Nodes/RobotNodeFixedFactory.cpp
index 54ba6e07b5242c11238fed7ee186867fa8170ce2..c78c8fc20d4aac1f5bf84e58b31ac751d6384dde 100644
--- a/VirtualRobot/Nodes/RobotNodeFixedFactory.cpp
+++ b/VirtualRobot/Nodes/RobotNodeFixedFactory.cpp
@@ -7,6 +7,7 @@
 #include "RobotNodeFixedFactory.h"
 #include "RobotNode.h"
 #include "RobotNodeFixed.h"
+#include "../CollisionDetection/CollisionModel.h"
 
 
 namespace VirtualRobot
diff --git a/VirtualRobot/Nodes/RobotNodePrismatic.cpp b/VirtualRobot/Nodes/RobotNodePrismatic.cpp
index a9a1c7c271bf8b2144f2c95f41b1338192bfff38..88c95894895715076b4d95c4f366da787646a5b0 100644
--- a/VirtualRobot/Nodes/RobotNodePrismatic.cpp
+++ b/VirtualRobot/Nodes/RobotNodePrismatic.cpp
@@ -1,11 +1,13 @@
 
 #include "RobotNodePrismatic.h"
 #include "../Robot.h"
+#include "../VirtualRobotException.h"
+#include "../CollisionDetection/CollisionModel.h"
+
 #include <cmath>
 #include <algorithm>
 
 #include <Eigen/Geometry>
-#include "../VirtualRobotException.h"
 
 namespace VirtualRobot
 {
diff --git a/VirtualRobot/Nodes/RobotNodePrismatic.h b/VirtualRobot/Nodes/RobotNodePrismatic.h
index 19c8f3220988b64f1ef3623c775e4efb799eb25b..9f7ff926d23fc019e92ad27b7fbc6d877e24f161 100644
--- a/VirtualRobot/Nodes/RobotNodePrismatic.h
+++ b/VirtualRobot/Nodes/RobotNodePrismatic.h
@@ -25,7 +25,6 @@
 #include "../VirtualRobot.h"
 
 #include "RobotNode.h"
-#include "../RobotFactory.h"
 
 #include <Eigen/Core>
 
diff --git a/VirtualRobot/Nodes/RobotNodePrismaticFactory.cpp b/VirtualRobot/Nodes/RobotNodePrismaticFactory.cpp
index f5cbeec7a9b8c9da5858c3718938dfbe8b700204..de8b34bfd700c3994aa7a9904b4d6c000389c4ce 100644
--- a/VirtualRobot/Nodes/RobotNodePrismaticFactory.cpp
+++ b/VirtualRobot/Nodes/RobotNodePrismaticFactory.cpp
@@ -7,6 +7,7 @@
 #include "RobotNodePrismaticFactory.h"
 #include "RobotNode.h"
 #include "RobotNodePrismatic.h"
+#include "../CollisionDetection/CollisionModel.h"
 
 
 namespace VirtualRobot
diff --git a/VirtualRobot/Nodes/RobotNodeRevolute.h b/VirtualRobot/Nodes/RobotNodeRevolute.h
index 6f02611dadf6f65af17f75db5e6c282eb6c9a941..37f1bfd79f862b4d362c5eae4838f41deca8178b 100644
--- a/VirtualRobot/Nodes/RobotNodeRevolute.h
+++ b/VirtualRobot/Nodes/RobotNodeRevolute.h
@@ -25,7 +25,6 @@
 #include "../VirtualRobot.h"
 
 #include "RobotNode.h"
-#include "../RobotFactory.h"
 
 #include <Eigen/Core>
 
diff --git a/VirtualRobot/Nodes/RobotNodeRevoluteFactory.cpp b/VirtualRobot/Nodes/RobotNodeRevoluteFactory.cpp
index 5364c87b13c55cf3ed32a51081dad06167279f63..344cfea475686c47754cba1862e8a532c5f621c8 100644
--- a/VirtualRobot/Nodes/RobotNodeRevoluteFactory.cpp
+++ b/VirtualRobot/Nodes/RobotNodeRevoluteFactory.cpp
@@ -7,6 +7,7 @@
 #include "RobotNodeRevoluteFactory.h"
 #include "RobotNode.h"
 #include "RobotNodeRevolute.h"
+#include "../CollisionDetection/CollisionModel.h"
 
 
 namespace VirtualRobot
diff --git a/VirtualRobot/Primitive.h b/VirtualRobot/Primitive.h
index 094f06a8d2b2f4aa189d709a59b03fc07d03b70d..5e27bf5fb68ffcf2ece4a3962f33c7c757027fb3 100644
--- a/VirtualRobot/Primitive.h
+++ b/VirtualRobot/Primitive.h
@@ -1,7 +1,8 @@
 #pragma once
 
 #include <VirtualRobot/VirtualRobot.h>
-#include <Eigen/Geometry>
+
+#include <Eigen/Core>
 
 
 namespace VirtualRobot
diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index f725ce465db7f0af97ebc10f64de2dccaee00225..13e4751c048ad8ee12e4498f6f0090c8181701a0 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -4,6 +4,7 @@
 #include "Trajectory.h"
 #include "VirtualRobotException.h"
 #include "CollisionDetection/CollisionChecker.h"
+#include "CollisionDetection/CollisionModel.h"
 #include "EndEffector/EndEffector.h"
 #include "math/Helpers.h"
 
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index 00d973b4bd5b225c7eb9cf52a7c889eaa06ad5e0..b494bfe93aef3cce2203a2e7574545be07eb7401 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -24,16 +24,13 @@
 
 #include "VirtualRobot.h"
 #include "SceneObject.h"
-#include "Nodes/Sensor.h"
 #include "Nodes/RobotNode.h"
-#include "Nodes/RobotNodeRevolute.h"
 #include "Nodes/ConditionedLock.h"
 
+#include <type_traits>
 #include <string>
-#include <map>
 #include <vector>
-
-#include <algorithm>
+#include <map>
 
 #include <Eigen/Core>
 
@@ -135,7 +132,6 @@ namespace VirtualRobot
         void setUpdateCollisionModel(bool enable) override;
 
         std::shared_ptr<Robot> shared_from_this() const;
-        //std::shared_ptr<Robot> shared_from_this() const { return boost::static_pointer_cast<Robot>(SceneObject::shared_from_this()); }
 
         /*!
             get the complete setup of all robot nodes
@@ -398,8 +394,6 @@ namespace VirtualRobot
 
         float getScaling();
         void setScaling(float scaling);
-        //float getRadianToMMfactor() const;
-        //void setRadianToMMfactor(float value);
 
         /**
          * @brief Inflates the collision models of all robot nodes of this robot. Useful for motion planning with a safety margin.
diff --git a/VirtualRobot/RobotFactory.cpp b/VirtualRobot/RobotFactory.cpp
index 6b0d09c206db4d834c1b466cf5dc6010ec9fe9e3..175c815cc899aef21756b13cec734d3eb6b81f37 100644
--- a/VirtualRobot/RobotFactory.cpp
+++ b/VirtualRobot/RobotFactory.cpp
@@ -7,6 +7,7 @@
 #include "Nodes/RobotNodePrismatic.h"
 #include "Nodes/RobotNodeFixed.h"
 #include "Nodes/RobotNodeFixedFactory.h"
+#include "CollisionDetection/CollisionModel.h"
 #include "EndEffector/EndEffector.h"
 #include "Visualization//VisualizationFactory.h"
 #include "VirtualRobotException.h"
diff --git a/VirtualRobot/Units.h b/VirtualRobot/Units.h
index a83ab59f5e6cd59f099d45e822a3e37758237996..1e628fa1d9ad37a16ce9ba26fb1bd873748735af 100644
--- a/VirtualRobot/Units.h
+++ b/VirtualRobot/Units.h
@@ -7,8 +7,8 @@
 */
 
 #include "VirtualRobotImportExport.h"
+
 #include <string>
-#include <algorithm>
 
 namespace VirtualRobot
 {
@@ -28,7 +28,6 @@ namespace VirtualRobot
 
         Units(const std::string& unitName) : unitString(unitName)
         {
-            //std::transform(unitString.begin(), unitString.end(), unitString.begin(), ::tolower);
         }
 
         bool isRadian()
diff --git a/VirtualRobot/Visualization/VisualizationFactory.h b/VirtualRobot/Visualization/VisualizationFactory.h
index 4ba837ab2cba6d22638c70b0d9d2dc90f142314d..1f40e951e70db52f031b341419bee7660a570eee 100644
--- a/VirtualRobot/Visualization/VisualizationFactory.h
+++ b/VirtualRobot/Visualization/VisualizationFactory.h
@@ -29,7 +29,6 @@
 #include "../Primitive.h"
 
 #include <Eigen/Core>
-#include <Eigen/Geometry>
 #include <string>
 
 namespace VirtualRobot
diff --git a/VirtualRobot/Visualization/VisualizationNode.cpp b/VirtualRobot/Visualization/VisualizationNode.cpp
index 6d9dcdf60fd4344efaf52801f5288bf30bd7d7fc..2ef58ef40eaa86e0ef50a7896b4a1da2f0f91b87 100644
--- a/VirtualRobot/Visualization/VisualizationNode.cpp
+++ b/VirtualRobot/Visualization/VisualizationNode.cpp
@@ -14,6 +14,8 @@
 #include "VirtualRobot/VirtualRobotException.h"
 #include "VirtualRobot/XML/BaseIO.h"
 
+#include <fstream>
+
 namespace VirtualRobot
 {
     using std::cout;
diff --git a/VirtualRobot/Visualization/VisualizationNode.h b/VirtualRobot/Visualization/VisualizationNode.h
index 9940e9b27173a67176156cbe151b519373a6f5a7..23572227ce16d110a6483bb60cfa34bc1bc52191 100644
--- a/VirtualRobot/Visualization/VisualizationNode.h
+++ b/VirtualRobot/Visualization/VisualizationNode.h
@@ -24,9 +24,10 @@
 
 #include "../VirtualRobot.h"
 #include "VisualizationFactory.h"
-#include <map>
+
 #include <Eigen/Core>
-#include <Eigen/Geometry>
+
+#include <map>
 
 namespace VirtualRobot
 {
diff --git a/VirtualRobot/XML/BaseIO.cpp b/VirtualRobot/XML/BaseIO.cpp
index 35db93d88486929688098c30ce2858600b006e0e..b01e929d56fea58322c1f4306caf07f7d3ab1d4c 100644
--- a/VirtualRobot/XML/BaseIO.cpp
+++ b/VirtualRobot/XML/BaseIO.cpp
@@ -6,6 +6,7 @@
 #include "../Trajectory.h"
 #include "../RuntimeEnvironment.h"
 #include "../VirtualRobotException.h"
+#include "../CollisionDetection/CollisionModel.h"
 #include "../EndEffector/EndEffector.h"
 #include "../EndEffector/EndEffectorActor.h"
 #include "../Nodes/RobotNodeFactory.h"
@@ -15,6 +16,7 @@
 #include "rapidxml.hpp"
 
 #include <filesystem>
+#include <fstream>
 
 namespace VirtualRobot
 {
diff --git a/VirtualRobot/XML/BaseIO.h b/VirtualRobot/XML/BaseIO.h
index 046ba0e62797b3bc27bda2e870eebd5a8406ac73..91a67de5a98ab04d658f83c94c221c6280f37dd1 100644
--- a/VirtualRobot/XML/BaseIO.h
+++ b/VirtualRobot/XML/BaseIO.h
@@ -24,16 +24,12 @@
 
 #include "../VirtualRobot.h"
 #include "../Units.h"
-#include "../MathTools.h"
-#include "../Robot.h"
 #include "../RobotConfig.h"
 #include "../Nodes/RobotNode.h"
-#include "../EndEffector/EndEffectorActor.h"
 
 #include <string>
 #include <vector>
 #include <map>
-#include <fstream>
 
 
 
diff --git a/VirtualRobot/XML/RobotIO.cpp b/VirtualRobot/XML/RobotIO.cpp
index 6125ed4697676291333c400faea4ae22c2e4be6b..37cf9a81b4f8571a8282bcfd9abd264aaa8e41f2 100644
--- a/VirtualRobot/XML/RobotIO.cpp
+++ b/VirtualRobot/XML/RobotIO.cpp
@@ -3,6 +3,7 @@
 #include "../RobotFactory.h"
 #include "../RobotNodeSet.h"
 #include "../VirtualRobotException.h"
+#include "../CollisionDetection/CollisionModel.h"
 #include "../EndEffector/EndEffector.h"
 #include "../EndEffector/EndEffectorActor.h"
 #include "../Nodes/RobotNodeFactory.h"
diff --git a/VirtualRobot/XML/RobotIO.h b/VirtualRobot/XML/RobotIO.h
index 2080f80bfb3be95578db0438ce1681238f04153a..f4d2cd0b84c8e01c756e90127f7aebecdd29bda2 100644
--- a/VirtualRobot/XML/RobotIO.h
+++ b/VirtualRobot/XML/RobotIO.h
@@ -23,16 +23,14 @@
 #pragma once
 
 #include "../VirtualRobot.h"
-#include "../Units.h"
-#include "../MathTools.h"
 #include "../Robot.h"
 #include "../Nodes/RobotNode.h"
+#include "../EndEffector/EndEffectorActor.h"
 #include "BaseIO.h"
 
 #include <string>
 #include <vector>
 #include <map>
-#include <fstream>
 
 
 
diff --git a/VirtualRobot/examples/RobotFromObjects/main.cpp b/VirtualRobot/examples/RobotFromObjects/main.cpp
index 1cf95bc14bf12554f499e72fce0461adf1aa0e61..446ee55737891f1caa93bdbc9306c11c740da4d6 100644
--- a/VirtualRobot/examples/RobotFromObjects/main.cpp
+++ b/VirtualRobot/examples/RobotFromObjects/main.cpp
@@ -5,9 +5,11 @@
 #include <VirtualRobot/Nodes/RobotNodeFixedFactory.h>
 #include <VirtualRobot/RuntimeEnvironment.h>
 #include <VirtualRobot/Robot.h>
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
 #include <VirtualRobot/Visualization/TriMeshModel.h>
 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
 #include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/RobotFactory.h>
 
 int main(int argc, char* argv[])
 {
diff --git a/VirtualRobot/examples/loadRobot/loadRobot.cpp b/VirtualRobot/examples/loadRobot/loadRobot.cpp
index 91b2c4edfc3144031c6211ad5dff0b27a59c8454..86e6a2acdc3c64d15c2f06c792c9c326fd45db5d 100644
--- a/VirtualRobot/examples/loadRobot/loadRobot.cpp
+++ b/VirtualRobot/examples/loadRobot/loadRobot.cpp
@@ -5,7 +5,7 @@
 #include <VirtualRobot/Transformation/DHParameter.h>
 #include <VirtualRobot/XML/RobotIO.h>
 #include <VirtualRobot/RuntimeEnvironment.h>
-
+#include <VirtualRobot/RobotFactory.h>
 
 #include <string>
 #include <iostream>
diff --git a/VirtualRobot/tests/VirtualRobotMathToolsTest.cpp b/VirtualRobot/tests/VirtualRobotMathToolsTest.cpp
index 76de034761aa05b55f98ad2014a23d390e5621f8..f2766dc2e63d9fca7cfa88a6f9e586ca3d00ecb1 100644
--- a/VirtualRobot/tests/VirtualRobotMathToolsTest.cpp
+++ b/VirtualRobot/tests/VirtualRobotMathToolsTest.cpp
@@ -9,6 +9,9 @@
 #include <VirtualRobot/VirtualRobotTest.h>
 #include <VirtualRobot/MathTools.h>
 #include <VirtualRobot/VirtualRobotException.h>
+
+#include <Eigen/Geometry>
+
 #include <string>
 
 #define M_PIf (static_cast<float>(M_PI))