Feature/subtrajectory

Implement getSubtrajectory for GlobalTrajectory and use it inside TimedElasticBand planning.

Also note #25 (closed) for the planToDest variable, as the free goal velocity when not planning to the end does not work yet.

Closes #13 (closed)

source/armarx/navigation/core/Trajectory.cpp

@@ -6,18 +6,6 @@
 #include <iterator>
 #include <limits>
 
-#include <range/v3/action/insert.hpp>
-#include <range/v3/numeric/accumulate.hpp>
-#include <range/v3/range/conversion.hpp>
-#include <range/v3/view/all.hpp>
-#include <range/v3/view/concat.hpp>
-#include <range/v3/view/for_each.hpp>
-#include <range/v3/view/group_by.hpp>
-#include <range/v3/view/reverse.hpp>
-#include <range/v3/view/sliding.hpp>
-#include <range/v3/view/transform.hpp>
-#include <range/v3/view/zip.hpp>
-
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
@@ -34,6 +22,17 @@
 #include <armarx/navigation/conversions/eigen.h>
 #include <armarx/navigation/core/basic_types.h>
 #include <armarx/navigation/core/types.h>
+#include <range/v3/action/insert.hpp>
+#include <range/v3/numeric/accumulate.hpp>
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/all.hpp>
+#include <range/v3/view/concat.hpp>
+#include <range/v3/view/for_each.hpp>
+#include <range/v3/view/group_by.hpp>
+#include <range/v3/view/reverse.hpp>
+#include <range/v3/view/sliding.hpp>
+#include <range/v3/view/transform.hpp>
+#include <range/v3/view/zip.hpp>
 
 // FIXME move to simox
 
@@ -158,13 +157,14 @@ namespace armarx::navigation::core
 
     } // namespace conv
 
-    Projection
-    GlobalTrajectory::getProjection(const Position& point,
-                              const VelocityInterpolation& velocityInterpolation) const
+
+    GlobalTrajectory::InternalProjection
+    GlobalTrajectory::projectInternal(const Position& point,
+                                      const VelocityInterpolation& velocityInterpolation) const
     {
         float distance = std::numeric_limits<float>::max();
 
-        Projection bestProj;
+        InternalProjection bestProj;
 
         for (size_t i = 0; i < pts.size() - 1; i++)
         {
@@ -198,26 +198,9 @@ namespace armarx::navigation::core
                 math::LinearInterpolatedPose ip(
                     wpBefore.waypoint.pose.matrix(), wpAfter.waypoint.pose.matrix(), 0, 1, true);
 
-                bestProj.wayPointBefore = wpBefore;
-                bestProj.wayPointAfter = wpAfter;
+                bestProj.indexBefore = i;
                 bestProj.projection.waypoint.pose = ip.Get(t);
 
-                bestProj.segment = [&]
-                {
-                    if (i == 0)
-                    {
-                        return Projection::Segment::FIRST;
-                    }
-
-                    if (i == (pts.size() - 2))
-                    {
-                        return Projection::Segment::FINAL;
-                    }
-
-                    return Projection::Segment::INTERMEDIATE;
-                }();
-
-
                 switch (velocityInterpolation)
                 {
                     case VelocityInterpolation::LinearInterpolation:
@@ -236,6 +219,82 @@ namespace armarx::navigation::core
         return bestProj;
     }
 
+    Projection
+    GlobalTrajectory::getProjection(const Position& point,
+                                    const VelocityInterpolation& velocityInterpolation) const
+    {
+        InternalProjection intProj = projectInternal(point, velocityInterpolation);
+
+        const auto& wpBefore = pts.at(intProj.indexBefore);
+        const auto& wpAfter = pts.at(intProj.indexBefore + 1);
+
+        Projection bestProj;
+
+        bestProj.wayPointBefore = wpBefore;
+        bestProj.wayPointAfter = wpAfter;
+        bestProj.projection = intProj.projection;
+
+        bestProj.segment = [&]
+        {
+            if (intProj.indexBefore == 0)
+            {
+                return Projection::Segment::FIRST;
+            }
+
+            if (intProj.indexBefore == (pts.size() - 2))
+            {
+                return Projection::Segment::FINAL;
+            }
+
+            return Projection::Segment::INTERMEDIATE;
+        }();
+
+        return bestProj;
+    }
+
+    std::pair<GlobalTrajectory, bool>
+    GlobalTrajectory::getSubTrajectory(const Position& point, float distance) const
+    {
+        const InternalProjection intProj =
+            projectInternal(point, VelocityInterpolation::LinearInterpolation);
+
+        GlobalTrajectory subTraj{};
+        subTraj.mutablePoints().push_back(intProj.projection);
+
+        float remainingDistance = distance;
+        GlobalTrajectoryPoint lastWp = intProj.projection;
+        for (size_t i = intProj.indexBefore + 1; i < pts.size(); i++)
+        {
+            const auto& nextWp = pts.at(i);
+
+            const float segmentDistance =
+                (nextWp.waypoint.pose.translation() - lastWp.waypoint.pose.translation()).norm();
+
+            if (segmentDistance < remainingDistance)
+            {
+                subTraj.mutablePoints().push_back(nextWp);
+                lastWp = nextWp;
+                remainingDistance -= segmentDistance;
+            }
+            else
+            {
+                // fraction of distance between segment end points
+                const float t = remainingDistance / segmentDistance;
+
+                math::LinearInterpolatedPose ip(
+                    lastWp.waypoint.pose.matrix(), nextWp.waypoint.pose.matrix(), 0, 1, true);
+                const float velocity = lastWp.velocity + (nextWp.velocity - lastWp.velocity) * t;
+
+                subTraj.mutablePoints().push_back({{static_cast<Pose>(ip.Get(t))}, velocity});
+
+                return {subTraj, false};
+            }
+        }
+
+        // remaining trajectory is shorter than the specified distance
+        return {subTraj, true};
+    }
+
     std::vector<Eigen::Vector3f>
     GlobalTrajectory::positions() const noexcept
     {
@@ -273,9 +332,9 @@ namespace armarx::navigation::core
 
     GlobalTrajectory
     GlobalTrajectory::FromPath(const Pose& start,
-                         const Positions& waypoints,
-                         const Pose& goal,
-                         const float velocity)
+                               const Positions& waypoints,
+                               const Pose& goal,
+                               const float velocity)
     {
         // currently, only 2D version
 
@@ -331,7 +390,8 @@ namespace armarx::navigation::core
     {
         core::Positions resampledPath;
 
-        const auto toPoint = [](const GlobalTrajectoryPoint& wp) -> Pose { return wp.waypoint.pose; };
+        const auto toPoint = [](const GlobalTrajectoryPoint& wp) -> Pose
+        { return wp.waypoint.pose; };
 
         const core::Path originalPath = pts | ranges::views::transform(toPoint) | ranges::to_vector;
 
@@ -601,7 +661,7 @@ namespace armarx::navigation::core
 
             constexpr int nSamples = 100;
 
-            for(int j = 0; j < nSamples; j++)
+            for (int j = 0; j < nSamples; j++)
             {
                 const float progress = static_cast<float>(j) / nSamples;
 
@@ -675,7 +735,6 @@ namespace armarx::navigation::core
         return indices;
     }
 
-
     const std::vector<LocalTrajectoryPoint>&
     LocalTrajectory::points() const
     {
@@ -714,14 +773,16 @@ namespace armarx::navigation::core
 
         // fraction of time between segment end points
         const float t = d1 / dT;
-        math::LinearInterpolatedPose ip(global_T_wp_before.matrix(), global_T_wp_after.matrix(), 0, 1, true);
+        math::LinearInterpolatedPose ip(
+            global_T_wp_before.matrix(), global_T_wp_after.matrix(), 0, 1, true);
 
         const core::Pose wp_before_T_wp_after = global_T_wp_before.inverse() * global_T_wp_after;
 
         const Eigen::Matrix3f& global_R_wp_before = global_T_wp_before.linear();
 
         // the movement direction in the global frame
-        const Eigen::Vector3f global_V_linear_movement_direction = global_R_wp_before * wp_before_T_wp_after.translation().normalized();
+        const Eigen::Vector3f global_V_linear_movement_direction =
+            global_R_wp_before * wp_before_T_wp_after.translation().normalized();
         const Eigen::Vector3f distance = lower[1].pose.translation() - lower->pose.translation();
         const float linearVelocity = static_cast<float>(distance.norm()) / dT.toSecondsDouble();
 
@@ -730,11 +791,9 @@ namespace armarx::navigation::core
 
         Eigen::AngleAxisf angleAx(wp_before_T_wp_after.linear());
 
-        const core::Twist feedforwardTwist
-        {
+        const core::Twist feedforwardTwist{
             .linear = linearVelocity * global_V_linear_movement_direction,
-            .angular = angleAx.axis() * angleAx.angle() / dT.toSecondsDouble()
-        };
+            .angular = angleAx.axis() * angleAx.angle() / dT.toSecondsDouble()};
 
         return {static_cast<core::Pose>(ip.Get(t)), feedforwardTwist};
     }