diff --git a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
index d149caed3a25294d59c55b61cd6d19294dcfc8b9..92b02612dd327bc9b80a09f5ec166e047139ccf4 100644
--- a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
+++ b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
@@ -43,6 +43,9 @@ namespace simox::geometric_planning
         const auto node = articulatedObject->getRobotNode(nodeName);
         REQUIRE(node != nullptr);
 
+        const auto global_T_object_root = articulatedObject->getGlobalPose();
+        articulatedObject->setGlobalPose(Eigen::Matrix4f::Identity());
+
         simox::geometric_planning::ArticulatedObjectGeometricPlanningHelper helper(
             articulatedObject);
 
@@ -84,6 +87,10 @@ namespace simox::geometric_planning
 
         const auto parametricPath = helper.getPathForNode(node->getName(), joint->getName());
 
+        // reset global pose
+        articulatedObject->setGlobalPose(global_T_object_root);
+
+
         return parametricPath;
     }
 
diff --git a/GeometricPlanning/ParametricPath.cpp b/GeometricPlanning/ParametricPath.cpp
index bd5e81e9e4cf9616508bb4c478e9a1de35b68744..9c9de69fdb92049c238af7594ed571f5fe241eac 100644
--- a/GeometricPlanning/ParametricPath.cpp
+++ b/GeometricPlanning/ParametricPath.cpp
@@ -19,6 +19,11 @@ namespace simox::geometric_planning
     {
         return path->progress(toLocalPathFrame(global_T_pose));
     }
+    
+    float ParametricPath::progress(const float param) const
+    {
+        return path->progress(param);
+    }
 
     Pose
     ParametricPath::toLocalPathFrame(const Pose& global_T_pose) const
diff --git a/GeometricPlanning/ParametricPath.h b/GeometricPlanning/ParametricPath.h
index ec58679d9b4d4e33f3a5eb5ce083641b9cd874be..dd2400f85fce57eef08bee5d698c98b6115f76ca 100644
--- a/GeometricPlanning/ParametricPath.h
+++ b/GeometricPlanning/ParametricPath.h
@@ -32,6 +32,7 @@ namespace simox::geometric_planning
 
         // helper functions to obtain the PathPrimitive's parameter and progress directly from a global pose
         float progress(const Pose& global_T_pose) const;
+        float progress(float param) const;
 
         Pose toLocalPathFrame(const Pose& global_T_pose) const;
 
diff --git a/GeometricPlanning/path_primitives/Circle.cpp b/GeometricPlanning/path_primitives/Circle.cpp
index 0f074f8c6bb5df4b267584e938a1706398cf20fd..55137eef3b4f4d3eca7c206b72c7a722ac08852f 100644
--- a/GeometricPlanning/path_primitives/Circle.cpp
+++ b/GeometricPlanning/path_primitives/Circle.cpp
@@ -23,7 +23,9 @@ namespace simox::geometric_planning
     Eigen::Vector3f
     Circle::getPosition(float t) const
     {
-        REQUIRE(parameterRange().isInRange(t));
+        // REQUIRE(parameterRange().isInRange(t));
+
+        t = clampParameter(t);
 
         return radius * Eigen::Vector3f(std::cos(t), std::sin(t), 0.0F);
     }
@@ -31,7 +33,8 @@ namespace simox::geometric_planning
     Eigen::Vector3f
     Circle::getPositionDerivative([[maybe_unused]] float t) const
     {
-        REQUIRE(parameterRange().isInRange(t));
+        // REQUIRE(parameterRange().isInRange(t));
+        
 
         // return radius * Eigen::Vector3f(-std::sin(t + M_PI_2f32), std::cos(t + M_PI_2f32), 0.0F);
 
@@ -80,7 +83,7 @@ namespace simox::geometric_planning
         const float param = phi;
         // ARMARX_DEBUG << "Param is " << param;
 
-        return std::clamp(param, parameterRange().min, parameterRange().max);
+        return clampParameter(param);
     }
 
 } // namespace simox::geometric_planning
diff --git a/GeometricPlanning/path_primitives/PathPrimitive.cpp b/GeometricPlanning/path_primitives/PathPrimitive.cpp
index d78fb0e528b454c1547fa548a596c3f4403a3c76..a00ada676a913c29d879a09eac87ac0371054b3c 100644
--- a/GeometricPlanning/path_primitives/PathPrimitive.cpp
+++ b/GeometricPlanning/path_primitives/PathPrimitive.cpp
@@ -16,9 +16,23 @@ namespace simox::geometric_planning
     PathPrimitive::progress(const Pose& pose) const
     {
         const float param = parameter(pose);
+        return progress(param);
+    }
+    
+    float PathPrimitive::progress(const float param) const
+    {
         const auto range = parameterRange();
 
-        return (param - range.min) / (range.max - range.min);
+        const float paramSanitized = clampParameter(param);
+
+        return (paramSanitized - range.min) / (range.max - range.min);
+    }
+
+
+    float PathPrimitive::clampParameter(float t) const
+    {
+        const auto range = parameterRange();
+        return std::clamp(t, range.min, range.max);
     }
 
     Pose
@@ -26,6 +40,7 @@ namespace simox::geometric_planning
     {
         return Pose(::math::Helpers::CreatePose(getPosition(t), getOrientation(t)));
     }
+    
 
     Eigen::Vector3f
     PathPrimitive::GetPosition(float t)
diff --git a/GeometricPlanning/path_primitives/PathPrimitive.h b/GeometricPlanning/path_primitives/PathPrimitive.h
index e5e81ae1120548cc4387cb777719f198598ecb68..48aa40203189c1085bb4d3b16256c5111a0c1199 100644
--- a/GeometricPlanning/path_primitives/PathPrimitive.h
+++ b/GeometricPlanning/path_primitives/PathPrimitive.h
@@ -36,6 +36,7 @@ namespace simox::geometric_planning
          * @return the progress in range [0,1]
          */
         float progress(const Pose& pose) const;
+        float progress(float param) const;
 
         // math::AbstractFunctionR1R6 interface
         Eigen::Vector3f GetPosition(float t) override;
@@ -51,6 +52,8 @@ namespace simox::geometric_planning
 
         Pose getPose(float t) const;
 
+        float clampParameter(float t) const;
+
         ~PathPrimitive() override = default;
     };