From dd33f3fc83cd886ede704c2a69e4c6914a316967 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Sun, 26 Feb 2023 10:25:25 +0100
Subject: [PATCH] geom planning update
---
GeometricPlanning/path_primitives/Circle.cpp | 9 ++++++---
GeometricPlanning/path_primitives/PathPrimitive.cpp | 8 ++++++++
GeometricPlanning/path_primitives/PathPrimitive.h | 2 ++
3 files changed, 16 insertions(+), 3 deletions(-)
diff --git a/GeometricPlanning/path_primitives/Circle.cpp b/GeometricPlanning/path_primitives/Circle.cpp
index 0f074f8c6..55137eef3 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 d78fb0e52..4c6e7d189 100644
--- a/GeometricPlanning/path_primitives/PathPrimitive.cpp
+++ b/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)
diff --git a/GeometricPlanning/path_primitives/PathPrimitive.h b/GeometricPlanning/path_primitives/PathPrimitive.h
index e5e81ae11..eff784793 100644
--- a/GeometricPlanning/path_primitives/PathPrimitive.h
+++ b/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;
};
--
GitLab