trajectory duration

source/armarx/navigation/core/Trajectory.cpp

@@ -585,8 +585,36 @@ namespace armarx::navigation::core
     float
     Trajectory::duration(const core::VelocityInterpolation interpolation) const
     {
-        throw LocalException("not implemented yet");
-        return 0; // FIXME
+
+        float dur = 0;
+
+        for (int i = 0; i < static_cast<int>(pts.size() - 1); i++)
+        {
+            const auto& start = pts.at(i);
+            const auto& goal = pts.at(i + 1);
+
+            const float distance =
+                (goal.waypoint.pose.translation() - start.waypoint.pose.translation()).norm();
+
+            const float startVelocity = start.velocity;
+            const float goalVelocity = goal.velocity;
+
+            constexpr int nSamples = 100;
+
+            for(int j = 0; j < nSamples; j++)
+            {
+                const float progress = static_cast<float>(j) / nSamples;
+
+                const float v = startVelocity + progress * (goalVelocity - startVelocity);
+                const float s = distance / nSamples;
+
+                const float t = s / v;
+
+                dur += t;
+            }
+        }
+
+        return dur;
     }
 
     bool

source/armarx/navigation/core/Trajectory.h

@@ -114,6 +114,7 @@ namespace armarx::navigation::core
          */
         Indices allConnectedPointsInRange(std::size_t idx, float radius) const;
 
+
     private:
         std::vector<TrajectoryPoint> pts;
     };