diff --git a/GeometricPlanning/ArticulatedObjectDoorHelper.cpp b/GeometricPlanning/ArticulatedObjectDoorHelper.cpp
index 87fd89839788dafb76c3d3bedd2ffa1ed2ac8e85..5ee378531046ced2ff8e13eb246dc6714e2c3646 100644
--- a/GeometricPlanning/ArticulatedObjectDoorHelper.cpp
+++ b/GeometricPlanning/ArticulatedObjectDoorHelper.cpp
@@ -14,13 +14,14 @@ namespace simox::geometric_planning
                                                              const Params& params) :
         object(object), params(params)
     {
+        CHECK_MESSAGE(object != nullptr, "Object must not be null");
     }
 
     ArticulatedObjectDoorHelper::DoorInteractionContext
     ArticulatedObjectDoorHelper::planInteraction(const std::string& nodeSetName) const
     {
         const auto rns = object->getRobotNodeSet(nodeSetName);
-        REQUIRE_MESSAGE(rns != nullptr, "Robot node set `" << nodeSetName << "` does not exist!");
+        CHECK_MESSAGE(rns != nullptr, std::string("Robot node set `" + nodeSetName + "` does not exist!"));
 
         const std::string jointNodeName = nodeSetName + constants::JointSuffix;
         const std::string handleNodeName = nodeSetName + constants::HandleSuffix;
@@ -28,9 +29,9 @@ namespace simox::geometric_planning
 
         const auto checkNodeExists = [&rns, &nodeSetName]([[maybe_unused]] const std::string& nodeName)
         {
-            REQUIRE_MESSAGE(rns->hasRobotNode(nodeName),
-                            "Robot node `" << nodeName + "` does not exist within robot node set `"
-                                           << nodeSetName << "`!");
+            CHECK_MESSAGE(rns->hasRobotNode(nodeName),
+                            std::string("Robot node `" + nodeName + "` does not exist within robot node set `"
+                                           + nodeSetName + "`!"));
         };
 
         for (const auto& nodeName : {jointNodeName, handleNodeName, surfaceProjectionNodeName})
@@ -38,7 +39,7 @@ namespace simox::geometric_planning
             checkNodeExists(nodeName);
         }
 
-        REQUIRE_MESSAGE(params.doorContactHandleDistance > 0.,
+        CHECK_MESSAGE(params.doorContactHandleDistance > 0.,
                         "Grasping from the other side not implemented yet!");
 
         return DoorInteractionContext{
diff --git a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
index 601c16348b897e454382a5ab833c5d0f3101827b..d149caed3a25294d59c55b61cd6d19294dcfc8b9 100644
--- a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
+++ b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
@@ -143,8 +143,8 @@ namespace simox::geometric_planning
 
         // ARMARX_DEBUG << "radius is " << radius;
 
-        Pose postTransform; //(joint_R_node);
-        postTransform.setIdentity(); // FIXME ???
+        // Pose postTransform; //(joint_R_node);
+        // postTransform.setIdentity(); // FIXME ???
 
         //
         auto subpart = articulatedObject;
@@ -193,7 +193,7 @@ namespace simox::geometric_planning
 
         return {jointReferenceNode,
                 std::make_unique<CircleSegment>(radius, parameterRange),
-                postTransform};
+                Pose::Identity()};
     }
 
     ParametricPath
@@ -263,7 +263,6 @@ namespace simox::geometric_planning
 
         return ParametricPath(jointReferenceNode,
                               std::make_unique<Line>(parameterRange),
-                              //   Pose(articulatedObject->getGlobalPose()),
                               Pose::Identity());
     }
 
diff --git a/GeometricPlanning/path_primitives/Circle.cpp b/GeometricPlanning/path_primitives/Circle.cpp
index 5a2a7620974f30c09f4297db31228c55d71ebe87..0f074f8c6bb5df4b267584e938a1706398cf20fd 100644
--- a/GeometricPlanning/path_primitives/Circle.cpp
+++ b/GeometricPlanning/path_primitives/Circle.cpp
@@ -37,7 +37,7 @@ namespace simox::geometric_planning
 
         // the position derivative is always pointing into x direction as orientation changes as well
         // FIXME update this according to getPosition!
-        return radius * -Eigen::Vector3f::UnitX();
+        return radius * Eigen::Vector3f::UnitY();
     }
 
     Eigen::Quaternionf