diff --git a/GraspPlanning/ApproachMovementGenerator.cpp b/GraspPlanning/ApproachMovementGenerator.cpp
index bb919b1e04a66e7b6a4f63ff4af4180c4020a25d..3d5093a60a26edfd84baec6cc9bd51c6942a7676 100644
--- a/GraspPlanning/ApproachMovementGenerator.cpp
+++ b/GraspPlanning/ApproachMovementGenerator.cpp
@@ -6,7 +6,9 @@
 #include <VirtualRobot/EndEffector/EndEffector.h>
 #include <VirtualRobot/MathTools.h>
 #include <VirtualRobot/RobotConfig.h>
+#include <VirtualRobot/math/Helpers.h>
 #include <iostream>
+
 using namespace std;
 
 namespace GraspStudio
@@ -53,7 +55,9 @@ namespace GraspStudio
     bool ApproachMovementGenerator::setEEFPose(const Eigen::Matrix4f& pose)
     {
         VirtualRobot::RobotNodePtr tcp;
-        if (!graspPreshape.empty() && eef_cloned->hasPreshape(graspPreshape) && eef_cloned->getPreshape(graspPreshape)->getTCP())
+        if (!graspPreshape.empty()
+            && eef_cloned->hasPreshape(graspPreshape)
+            && eef_cloned->getPreshape(graspPreshape)->getTCP())
             tcp = eef_cloned->getPreshape(graspPreshape)->getTCP();
         else
             tcp = eef_cloned->getGCP();
@@ -63,10 +67,9 @@ namespace GraspStudio
 
     bool ApproachMovementGenerator::updateEEFPose(const Eigen::Vector3f& deltaPosition)
     {
-        Eigen::Matrix4f deltaPose;
-        deltaPose.setIdentity();
-        deltaPose.block(0, 3, 3, 1) = deltaPosition;
-        return updateEEFPose(deltaPose);
+        Eigen::Matrix4f pose = eef_cloned->getGCP()->getGlobalPose();
+        math::Helpers::Position(pose) += deltaPosition;
+        return setEEFPose(pose);
     }
 
     bool ApproachMovementGenerator::updateEEFPose(const Eigen::Matrix4f& deltaPose)
diff --git a/GraspPlanning/ApproachMovementSurfaceNormal.cpp b/GraspPlanning/ApproachMovementSurfaceNormal.cpp
index 921ea0d7bbd9b65a3cfaf0f851a2c3226fd48ac8..d5264e9e595d10a1e0a3b99e1e7fa27254ad666a 100644
--- a/GraspPlanning/ApproachMovementSurfaceNormal.cpp
+++ b/GraspPlanning/ApproachMovementSurfaceNormal.cpp
@@ -60,6 +60,11 @@ namespace GraspStudio
           position(2) = (objectModel->vertices[nVert1].z + objectModel->vertices[nVert2].z + objectModel->vertices[nVert3].z) / 3.0f;*/
         
         storeApproachDir = (objectModel->faces[faceIndex]).normal;
+        if (std::abs(storeApproachDir.squaredNorm() - 1) > 1e-6f)
+        {
+            std::cout << "Normal in trimesh not normalized!";
+            storeApproachDir.normalize();
+        }
         
         return true;
     }
@@ -72,6 +77,7 @@ namespace GraspStudio
         Eigen::Vector3f position;
         Eigen::Vector3f approachDir;
 
+        
         if (!getPositionOnObject(position, approachDir))
         {
             GRASPSTUDIO_ERROR << "no position on object?!" << endl;
@@ -81,7 +87,7 @@ namespace GraspStudio
         this->approachDirGlobal = approachDir;
 
         // set new pose
-        setEEFToApproachPose(position,approachDir);
+        setEEFToApproachPose(position, approachDir);
 
 
         // move away until valid
@@ -105,11 +111,12 @@ namespace GraspStudio
 
         // restore original pose
         setEEFPose(pose);
-
+        
         return poseB;
     }
 
-    bool ApproachMovementSurfaceNormal::setEEFToApproachPose(const Eigen::Vector3f& position, const Eigen::Vector3f& approachDir)
+    bool ApproachMovementSurfaceNormal::setEEFToApproachPose(
+            const Eigen::Vector3f& position, const Eigen::Vector3f& approachDir)
     {
         VirtualRobot::RobotNodePtr graspNode = eef_cloned->getGCP();
 
@@ -188,22 +195,23 @@ namespace GraspStudio
         poseFinal.block(0, 2, 3, 1) = z;
 
         setEEFPose(poseFinal);
+        
         return true;
     }
 
-    void ApproachMovementSurfaceNormal::moveEEFAway(const Eigen::Vector3f& approachDir, float step, int maxLoops)
+    void ApproachMovementSurfaceNormal::moveEEFAway(
+            const Eigen::Vector3f& approachDir, float step, int maxLoops)
     {
-        VirtualRobot::SceneObjectSetPtr sos = eef_cloned->createSceneObjectSet();
+        VirtualRobot::SceneObjectSetPtr sceneObjectSet = eef_cloned->createSceneObjectSet();
+        if (!sceneObjectSet) return;
 
-        if (!sos)
-        {
-            return;
-        }
+        CollisionModelPtr objectColModel = object->getCollisionModel();
+        CollisionCheckerPtr eefCollChecker = eef_cloned->getCollisionChecker();
 
-        int loop = 0;
         Eigen::Vector3f delta = approachDir * step;
-
-        while (loop < maxLoops && eef_cloned->getCollisionChecker()->checkCollision(object->getCollisionModel(), sos))
+        int loop = 0;
+        
+        while (loop < maxLoops && eefCollChecker->checkCollision(objectColModel, sceneObjectSet))
         {
             updateEEFPose(delta);
             loop++;
diff --git a/SimDynamics/DynamicsWorld.cpp b/SimDynamics/DynamicsWorld.cpp
index 9d4f99b603de0e0ce92f2f549c3b70571c75531d..9970ac93afa5f7dd5d043436749706acda68aee2 100644
--- a/SimDynamics/DynamicsWorld.cpp
+++ b/SimDynamics/DynamicsWorld.cpp
@@ -74,7 +74,7 @@ namespace SimDynamics
 
     DynamicsWorld::DynamicsWorld(DynamicsEngineConfigPtr config)
     {
-        DynamicsEngineFactoryPtr factory = DynamicsEngineFactory::first(NULL);
+        DynamicsEngineFactoryPtr factory = DynamicsEngineFactory::first(nullptr);
         THROW_VR_EXCEPTION_IF(!factory, "No Physics Engine Found. Re-Compile with engine support...");
         engine = factory->createEngine(config);
         THROW_VR_EXCEPTION_IF(!engine, "Could not create Physics Engine.");
@@ -97,13 +97,15 @@ namespace SimDynamics
     {
         SIMDYNAMICS_ASSERT(o);
 
-        DynamicsEngineFactoryPtr factory = DynamicsEngineFactory::first(NULL);
+        DynamicsEngineFactoryPtr factory = DynamicsEngineFactory::first(nullptr);
         SIMDYNAMICS_ASSERT(factory);
 
         return factory->createObject(o);
     }
 
-    void DynamicsWorld::createFloorPlane(const Eigen::Vector3f& pos /*= Eigen::Vector3f(0,0,0)*/, const Eigen::Vector3f& up /*= Eigen::Vector3f(0,0,1.0f)*/, float friction)
+    void DynamicsWorld::createFloorPlane(const Eigen::Vector3f& pos /*= Eigen::Vector3f(0,0,0)*/,
+                                         const Eigen::Vector3f& up /*= Eigen::Vector3f(0,0,1.0f)*/,
+                                         float friction)
     {
         engine->createFloorPlane(pos, up, friction);
     }
@@ -122,7 +124,7 @@ namespace SimDynamics
     {
         SIMDYNAMICS_ASSERT(rob);
 
-        DynamicsEngineFactoryPtr factory = DynamicsEngineFactory::first(NULL);
+        DynamicsEngineFactoryPtr factory = DynamicsEngineFactory::first(nullptr);
         SIMDYNAMICS_ASSERT(factory);
 
         return factory->createRobot(rob);