fix: only optimizing orientation if intermediate points are available

source/armarx/navigation/global_planning/AStar.cpp

@@ -164,7 +164,8 @@ namespace armarx::navigation::global_planning
 
         // planner.setRobotColModel("Platform-colmodel");
         //planner.setRobotCollisionModel(robotCollisionModel());
-        planner.setRobotCollisionModel(scene.robot->getRobotNode("Platform-navigation-colmodel")->getCollisionModel());
+        planner.setRobotCollisionModel(
+            scene.robot->getRobotNode("Platform-navigation-colmodel")->getCollisionModel());
 
         const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
 
@@ -267,6 +268,12 @@ namespace armarx::navigation::global_planning
 
         // ARMARX_CHECK(resampledTrajectory.hasMaxDistanceBetweenWaypoints(params.resampleDistance));
 
+        if (resampledTrajectory->points().size() == 2)
+        {
+            ARMARX_INFO << "Only start and goal provided. Not optimizing orientation";
+            ARMARX_TRACE;
+            return GlobalPlannerResult{.trajectory = resampledTrajectory.value()};
+        }
 
         ARMARX_TRACE;
         OrientationOptimizer optimizer(resampledTrajectory.value(), OrientationOptimizer::Params{});