geom planning update

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

GeometricPlanning/path_primitives/PathPrimitive.cpp

@@ -21,11 +21,19 @@ namespace simox::geometric_planning
         return (param - 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
     PathPrimitive::getPose(const float t) const
     {
         return Pose(::math::Helpers::CreatePose(getPosition(t), getOrientation(t)));
     }
+    
 
     Eigen::Vector3f
     PathPrimitive::GetPosition(float t)

GeometricPlanning/path_primitives/PathPrimitive.h

@@ -51,6 +51,8 @@ namespace simox::geometric_planning
 
         Pose getPose(float t) const;
 
+        float clampParameter(float t) const;
+
         ~PathPrimitive() override = default;
     };