path progress

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

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;
 

GeometricPlanning/path_primitives/PathPrimitive.cpp

@@ -16,9 +16,16 @@ 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);
     }
 
 

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;