diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp
index 90842a6..49b4214 100644
--- a/src/modelec_strat/src/pathfinding.cpp
+++ b/src/modelec_strat/src/pathfinding.cpp
@@ -529,6 +529,7 @@ namespace Modelec
             else
             {
                 wp.theta = goal->theta;
+                wp.x += goal->x;
             }
 
             wp.id = id++;
@@ -540,11 +541,6 @@ namespace Modelec
             waypoints.back().is_end = true;
         }
 
-        for (auto & wp : waypoints)
-        {
-            wp.x += 1;
-        }
-
         return {FREE, waypoints};
     }