after discussion with Christian: quadratic cost for obstacle distance; revised...

after discussion with Christian: quadratic cost for obstacle distance; revised the formula to compute the costs

source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp

@@ -196,19 +196,19 @@ namespace armarx::navigation::algorithms::spfa
                         continue;
 
 
-                    // const float obstacleDistance1 = std::min(inputMap(row, col), 1000.F);
-                    const float obstacleDistance2 =
+                    const float clippedObstacleDistance =
                         std::min(inputMap(ip, jp), params.obstacleMaxDistance);
 
                     const float travelCost = dir_lengths[k];
-                    // const float obstacleDistanceCost = std::max(obstacleDistance1 - obstacleDistance2, 0.F) / costmap.params().cellSize;
+
                     const float targetDistanceCost =
                         params.obstacleDistanceWeight *
-                        (params.obstacleMaxDistance - obstacleDistance2) /
-                        params.obstacleMaxDistance;
+                        std::pow(1.F - clippedObstacleDistance / params.obstacleMaxDistance,
+                                 params.obstacleCostExponent);
 
-                    const float edgeCost =
-                        params.obstacleDistanceCosts ? travelCost + targetDistanceCost : travelCost;
+                    const float edgeCost = params.obstacleDistanceCosts
+                                               ? travelCost * (1 + targetDistanceCost)
+                                               : travelCost;
 
                     int e = ravel(v, edge_counts[v], max_edges_per_vert);
                     edges[e] = vp;

source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h

@@ -40,6 +40,8 @@ namespace armarx::navigation::algorithms::spfa
             float obstacleMaxDistance = 1000.F;
 
             float obstacleDistanceWeight = 1.5F;
+
+            float obstacleCostExponent = 2.F;
         };
 
         ShortestPathFasterAlgorithm(const Costmap& costmap, const Parameters& params);

source/armarx/navigation/global_planning/SPFA.cpp

@@ -156,8 +156,7 @@ namespace armarx::navigation::global_planning
         // smoothPlan.pop_back();
 
         ARMARX_TRACE;
-        auto trajectory =
-            core::Trajectory::FromPath(start, wpts, goal, params.linearVelocity);
+        auto trajectory = core::Trajectory::FromPath(start, wpts, goal, params.linearVelocity);
 
         // TODO(fabian.reister): resampling of trajectory
 
@@ -211,8 +210,30 @@ namespace armarx::navigation::global_planning
 
         algorithm::CircularPathSmoothing smoothing;
         auto smoothTrajectory = smoothing.smooth(result.trajectory.value());
+
+
         smoothTrajectory.setMaxVelocity(params.linearVelocity);
 
+        const auto costmap = scene.staticScene->costmap.get();
+
+
+        for (auto& point : smoothTrajectory.mutablePoints())
+        {
+            const float distance = std::min<float>(
+                spfaParams.obstacleMaxDistance,
+                costmap->value(Eigen::Vector2f{point.waypoint.pose.translation().head<2>()})
+                    .value_or(0.F));
+
+            if (spfaParams.obstacleDistanceCosts)
+            {
+                point.velocity = params.linearVelocity /
+                                 (1.F + spfaParams.obstacleDistanceWeight *
+                                            std::pow(1 - distance / spfaParams.obstacleMaxDistance,
+                                                     spfaParams.obstacleCostExponent));
+            }
+        }
+
+
         ARMARX_TRACE;
         return GlobalPlannerResult{.trajectory = result.trajectory.value()};
     }

source/armarx/navigation/global_planning/SPFA.h

@@ -22,6 +22,7 @@
 
 #pragma once
 
+#include <VirtualRobot/MathTools.h>
 #include "GlobalPlanner.h"
 #include <armarx/navigation/core/types.h>
 
@@ -35,6 +36,13 @@ namespace armarx::navigation::global_planning
     struct SPFAParams : public GlobalPlannerParams
     {
         float linearVelocity{500};
+        float angularVelocity{2.F * M_PIf32 / 30.F };
+
+        //  core::TwistLimits limits{
+        //     .linear = 500.F,                // [mm/s]
+        //     .angular = 2.F * M_PIf32 / 30.F // [rad/s]
+        // };
+
         float resampleDistance{-1};