diff --git a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp
index 0626c8a661b847974cc1ff1ea63afd6623397e82..9bf44e7e7c380b50218f5c50cb512a4d546c7176 100644
--- a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp
+++ b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp
@@ -1,9 +1,11 @@
-#include <Eigen/Geometry>
 #include "GraspEvaluationPoseUncertainty.h"
 #include <VirtualRobot/Random.h>
 #include <VirtualRobot/math/Helpers.h>
 #include <VirtualRobot/math/ClampedNormalDistribution.hpp>
 
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
 #include <algorithm>
 #include <random>
 #include <cfloat>
diff --git a/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp b/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp
index be18acb9294f72df2814bb1bd8fdda136efc6371..7d04ffdc1549d2135a5730293cd6429562bb6e04 100644
--- a/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp
+++ b/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp
@@ -17,6 +17,7 @@
 #include "MotionPlanning/PostProcessing/ShortcutProcessor.h"
 #include <MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h>
 #include <QFileDialog>
+#include <Eigen/Dense>
 #include <Eigen/Geometry>
 #include <ctime>
 #include <vector>
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
index 755bb027dd3fa17e9ee48fe21e248f68c8917518..edf0cae4b265e67848f64104183e4b8c67412ab8 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
@@ -16,6 +16,8 @@
 #include <BulletCollision/CollisionShapes/btCylinderShape.h>
 #include <BulletCollision/CollisionShapes/btCompoundShape.h>
 
+#include <Eigen/Dense>
+
 //#define DEBUG_FIXED_OBJECTS
 //#define USE_BULLET_GENERIC_6DOF_CONSTRAINT
 
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
index 4b5cff4e94d679129bc5db2ac94cd69dca6ab1f5..9d91237304006d0ef87d3e0d6cd6aef01b97f101 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
@@ -17,6 +17,8 @@
 
 #include <boost/math/special_functions/fpclassify.hpp>
 
+#include <Eigen/Dense>
+
 #include <unordered_set>
 
 //#define DEBUG_FIXED_OBJECTS
diff --git a/VirtualRobot/Grasping/Grasp.cpp b/VirtualRobot/Grasping/Grasp.cpp
index 82d0da061327fe3d7ebf5782449df68aa0d6ba4a..e016a37133c9f8aa57e43db8e8ca6a53b2c235ad 100644
--- a/VirtualRobot/Grasping/Grasp.cpp
+++ b/VirtualRobot/Grasping/Grasp.cpp
@@ -3,6 +3,9 @@
 #include "../RobotConfig.h"
 #include "..//Robot.h"
 #include "../VirtualRobotException.h"
+
+#include <Eigen/Dense>
+
 #include <iomanip>
 
 namespace VirtualRobot
diff --git a/VirtualRobot/IK/DifferentialIK.h b/VirtualRobot/IK/DifferentialIK.h
index f8f847e2b73d4c8a578a7700eccac8b7cd7fd9f3..88eef461163d84c39a1d98b18a8fe1857b517bb6 100644
--- a/VirtualRobot/IK/DifferentialIK.h
+++ b/VirtualRobot/IK/DifferentialIK.h
@@ -30,6 +30,9 @@
 #include "JacobiProvider.h"
 #include "IKSolver.h"
 
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
 #include <string>
 #include <vector>
 
diff --git a/VirtualRobot/IK/JacobiProvider.cpp b/VirtualRobot/IK/JacobiProvider.cpp
index 169056b97083c27f48e9f71aa0cedd763faf727c..c8be128c7531976be64d46ba46cd77e64891c616 100644
--- a/VirtualRobot/IK/JacobiProvider.cpp
+++ b/VirtualRobot/IK/JacobiProvider.cpp
@@ -1,7 +1,7 @@
 #include <Eigen/Geometry>
 #include "JacobiProvider.h"
 
-
+#include <Eigen/Dense>
 
 #include <algorithm>
 
diff --git a/VirtualRobot/IK/constraints/OrientationConstraint.cpp b/VirtualRobot/IK/constraints/OrientationConstraint.cpp
index 0abee7d500417cf8ab01ab011515042e6c73085b..66298e115fb4d15171720dd176cb870fbbbb04f6 100644
--- a/VirtualRobot/IK/constraints/OrientationConstraint.cpp
+++ b/VirtualRobot/IK/constraints/OrientationConstraint.cpp
@@ -24,6 +24,8 @@
 #include <stdexcept>
 #include <sstream>
 
+#include <Eigen/Dense>
+
 #include "OrientationConstraint.h"
 
 #include <VirtualRobot/Robot.h>
diff --git a/VirtualRobot/IK/constraints/PoseConstraint.cpp b/VirtualRobot/IK/constraints/PoseConstraint.cpp
index 2d88b0e50311812d7da4a1c5c3b093b8da324746..c88cbf5a89418630897960e797d0ac49001804b8 100644
--- a/VirtualRobot/IK/constraints/PoseConstraint.cpp
+++ b/VirtualRobot/IK/constraints/PoseConstraint.cpp
@@ -25,6 +25,8 @@
 
 #include <VirtualRobot/Robot.h>
 
+#include <Eigen/Dense>
+
 #include <iostream>
 
 using namespace VirtualRobot;
diff --git a/VirtualRobot/IK/constraints/TSRConstraint.cpp b/VirtualRobot/IK/constraints/TSRConstraint.cpp
index c8d3a5b19cd66b7fdf79f3169eca3669df6582f4..882a1d137c1eb341db76662529472fb952708310 100644
--- a/VirtualRobot/IK/constraints/TSRConstraint.cpp
+++ b/VirtualRobot/IK/constraints/TSRConstraint.cpp
@@ -25,6 +25,8 @@
 
 #include <VirtualRobot/MathTools.h>
 
+#include <Eigen/Dense>
+
 using namespace VirtualRobot;
 
 #define POSITION_COMPONENT      0
diff --git a/VirtualRobot/LinkedCoordinate.cpp b/VirtualRobot/LinkedCoordinate.cpp
index ceba0c6da039ea6b338d47cdf0286d353a5d1973..3cd3c508837c1ecbb0f4ae53a0d2c55be89a5a2f 100644
--- a/VirtualRobot/LinkedCoordinate.cpp
+++ b/VirtualRobot/LinkedCoordinate.cpp
@@ -3,6 +3,8 @@
 
 #include <boost/format.hpp>
 
+#include <Eigen/Dense>
+
 
 using namespace VirtualRobot;
 using namespace std;
diff --git a/VirtualRobot/Nodes/RobotNode.h b/VirtualRobot/Nodes/RobotNode.h
index 5d007274283682513afd1d2e69f56f0101521438..88f6acf96e718a4075615a2ab4f36f46f390f5b3 100644
--- a/VirtualRobot/Nodes/RobotNode.h
+++ b/VirtualRobot/Nodes/RobotNode.h
@@ -23,7 +23,7 @@
 #pragma once
 
 #include "../VirtualRobot.h"
-
+#include "../VirtualRobotException.h"
 #include "../SceneObject.h"
 #include "../Transformation/DHParameter.h"
 #include "../Visualization/VisualizationNode.h"
diff --git a/VirtualRobot/Nodes/RobotNodeRevolute.cpp b/VirtualRobot/Nodes/RobotNodeRevolute.cpp
index 72f7a1134e5e51654cdafadb101c8c16f7aaa910..dc8267915956d492e4a41878af5af6a1f82d54a1 100644
--- a/VirtualRobot/Nodes/RobotNodeRevolute.cpp
+++ b/VirtualRobot/Nodes/RobotNodeRevolute.cpp
@@ -1,5 +1,6 @@
 
 #include "RobotNodeRevolute.h"
+#include "Nodes/Sensor.h"
 #include "../Robot.h"
 #include <cmath>
 #include <algorithm>
diff --git a/VirtualRobot/Nodes/Sensor.h b/VirtualRobot/Nodes/Sensor.h
index 803343adaf1e83c894bedc96d6f646c62f87b0d7..8e65e25c1815844decf7bdcaa93fc746da77c8fc 100644
--- a/VirtualRobot/Nodes/Sensor.h
+++ b/VirtualRobot/Nodes/Sensor.h
@@ -24,15 +24,10 @@
 
 #include "../VirtualRobot.h"
 #include "../VirtualRobotException.h"
-#include "../VirtualRobot.h"
 
 #include "../SceneObject.h"
-#include "../Visualization/VisualizationNode.h"
 
 #include <Eigen/Core>
-#include <Eigen/Geometry>
-
-
 
 #include <string>
 #include <vector>
diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index 13e4751c048ad8ee12e4498f6f0090c8181701a0..7c550309c5f9ca16f7aafac57681db3d2541b1e5 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -3,6 +3,7 @@
 #include "RobotConfig.h"
 #include "Trajectory.h"
 #include "VirtualRobotException.h"
+#include "Nodes/Sensor.h"
 #include "CollisionDetection/CollisionChecker.h"
 #include "CollisionDetection/CollisionModel.h"
 #include "EndEffector/EndEffector.h"
diff --git a/VirtualRobot/VirtualRobot.h b/VirtualRobot/VirtualRobot.h
index bf48e18bb729b5d6c8539fc84ecd842e5d43069a..7a01180f61d4af9acca8859ccdf445c85f0e6950 100644
--- a/VirtualRobot/VirtualRobot.h
+++ b/VirtualRobot/VirtualRobot.h
@@ -161,6 +161,7 @@ namespace VirtualRobot
     class WorkspaceDataArray;
     class ForceTorqueSensor;
     class ContactSensor;
+    class Sensor;
     class LocalRobot;
 
     typedef std::shared_ptr<CoMIK> CoMIKPtr;
@@ -211,6 +212,7 @@ namespace VirtualRobot
     typedef std::shared_ptr<WorkspaceGrid> WorkspaceGridPtr;
     typedef std::shared_ptr<ForceTorqueSensor> ForceTorqueSensorPtr;
     typedef std::shared_ptr<ContactSensor> ContactSensorPtr;
+    typedef std::shared_ptr<Sensor> SensorPtr;
     typedef std::shared_ptr<LocalRobot> LocalRobotPtr;
 
     /*