From df8d8acfea88874c037ba000e5f91a0c66396f82 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 2 Mar 2023 16:53:40 +0100
Subject: [PATCH] path progress
---
GeometricPlanning/ParametricPath.cpp | 5 +++++
GeometricPlanning/ParametricPath.h | 1 +
GeometricPlanning/path_primitives/PathPrimitive.cpp | 9 ++++++++-
GeometricPlanning/path_primitives/PathPrimitive.h | 1 +
4 files changed, 15 insertions(+), 1 deletion(-)
diff --git a/GeometricPlanning/ParametricPath.cpp b/GeometricPlanning/ParametricPath.cpp
index bd5e81e9e..9c9de69fd 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 ec58679d9..dd2400f85 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/PathPrimitive.cpp b/GeometricPlanning/path_primitives/PathPrimitive.cpp
index 4c6e7d189..a00ada676 100644
--- a/GeometricPlanning/path_primitives/PathPrimitive.cpp
+++ b/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);
}
diff --git a/GeometricPlanning/path_primitives/PathPrimitive.h b/GeometricPlanning/path_primitives/PathPrimitive.h
index eff784793..48aa40203 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;
--
GitLab