diff --git a/VirtualRobot/Safety.h b/VirtualRobot/Safety.h
index 57926f34895c650a63763edf5d4610eefe954c2a..e48e6af86bdbfd9691f8e09e90e0e30a718eec94 100644
--- a/VirtualRobot/Safety.h
+++ b/VirtualRobot/Safety.h
@@ -27,7 +27,7 @@ namespace VirtualRobot
                        nodes.end(),
                        std::back_inserter(nodePositions),
                        [](const RobotNodePtr& node)
-                       { return node->getPoseInRootFrame(); });
+                       { return node->getPositionInRootFrame().head<2>(); });
 
         const MathTools::ConvexHull2DPtr hull =
             MathTools::createConvexHull2D(nodePositions);