diff --git a/source/armarx/navigation/global_planning/AStar.cpp b/source/armarx/navigation/global_planning/AStar.cpp
index be670e2486fd1679c9f857ddde92e681e4acdebe..1d13ae8a45b26aefea53a67601907237c0d53c6c 100644
--- a/source/armarx/navigation/global_planning/AStar.cpp
+++ b/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{});