From 76c755e3c1c6f880f83e9e25af5e0083c92727d2 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 7 Jan 2022 13:54:04 +0100
Subject: [PATCH] trajectory: setting velocity properly after resampling
---
source/armarx/navigation/core/Trajectory.cpp | 41 +++++++++++++-------
source/armarx/navigation/core/Trajectory.h | 2 +
2 files changed, 29 insertions(+), 14 deletions(-)
diff --git a/source/armarx/navigation/core/Trajectory.cpp b/source/armarx/navigation/core/Trajectory.cpp
index de68a826..f68d2942 100644
--- a/source/armarx/navigation/core/Trajectory.cpp
+++ b/source/armarx/navigation/core/Trajectory.cpp
@@ -445,11 +445,7 @@ namespace armarx::navigation::core
<< "The resampled trajectory is only allowed to contains no points if it is "
"shorter than eps";
- return Trajectory::FromPath(
- pts.front().waypoint.pose,
- {}, // no intermediate points
- pts.back().waypoint.pose,
- 500.F); // TODO remove 0.F!, use proper interpolation! (via projection!)
+ return Trajectory({pts.front(), pts.back()});
}
@@ -492,22 +488,33 @@ namespace armarx::navigation::core
ranges::to_vector;
- const auto resampledTrajectory = Trajectory::FromPath(
- pts.front().waypoint.pose,
- resampledPath,
- pts.back().waypoint.pose,
- 500.F); // TODO remove 0.F!, use proper interpolation! (via projection!)
+ auto resampledTrajectory = Trajectory::FromPath(
+ pts.front().waypoint.pose, resampledPath, pts.back().waypoint.pose, 0.F);
+
+ // set velocity based on original trajectory
+ {
+ const auto setVelocityInPlace = [&](TrajectoryPoint& pt)
+ {
+ const auto projection = getProjection(pt.waypoint.pose.translation(),
+ VelocityInterpolation::LinearInterpolation);
+ pt.velocity = projection.projection.velocity;
+ };
+
+ std::for_each(resampledTrajectory.mutablePoints().begin(),
+ resampledTrajectory.mutablePoints().end(),
+ setVelocityInPlace);
+ }
// sanity check: make sure that resampling was successful
// number of samples required for source trajectory
- const float minRequiredSamples = length() / eps;
- const size_t samples = resampledTrajectory.points().size();
+ // const float minRequiredSamples = length() / eps;
+ // const size_t samples = resampledTrajectory.points().size();
- ARMARX_CHECK_GREATER_EQUAL(samples, minRequiredSamples);
+ // ARMARX_CHECK_GREATER_EQUAL(samples, minRequiredSamples);
// this is a rough approximation of the upper bound
- ARMARX_CHECK_LESS_EQUAL(samples, 2 * minRequiredSamples);
+ // ARMARX_CHECK_LESS_EQUAL(samples, 2 * minRequiredSamples);
return resampledTrajectory;
}
@@ -568,6 +575,12 @@ namespace armarx::navigation::core
return pts;
}
+ std::vector<TrajectoryPoint>&
+ Trajectory::mutablePoints()
+ {
+ return pts;
+ }
+
float
Trajectory::duration(const core::VelocityInterpolation interpolation) const
diff --git a/source/armarx/navigation/core/Trajectory.h b/source/armarx/navigation/core/Trajectory.h
index 4183d1cc..230ed746 100644
--- a/source/armarx/navigation/core/Trajectory.h
+++ b/source/armarx/navigation/core/Trajectory.h
@@ -97,6 +97,8 @@ namespace armarx::navigation::core
const std::vector<TrajectoryPoint>& points() const;
+ std::vector<TrajectoryPoint>& mutablePoints();
+
float duration(core::VelocityInterpolation interpolation) const;
private:
--
GitLab