diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
index a873f8c..f7b1786 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
+++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
@@ -404,37 +404,44 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     }
     
   }
-  else
+  else if(compute_plan_.poses.size() == 1)
   {
     try
     {
-      auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
-      auto prev_carrot_pose_it = transform_plan_.poses.begin();
-      double distance_it = 0;
-      for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it)
-      {
-        double dx = it->pose.x - carrot_pose_it->pose.x;
-        double dy = it->pose.y - carrot_pose_it->pose.y;
-        distance_it += std::hypot(dx, dy);
-        if (distance_it > costmap_robot_->getCostmap()->getResolution())
-        {
-          prev_carrot_pose_it = it;
-          break;
-        }
-      }
+      // auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
+      // auto prev_carrot_pose_it = transform_plan_.poses.begin();
+      // double distance_it = 0;
+      // for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it)
+      // {
+      //   double dx = it->pose.x - carrot_pose_it->pose.x;
+      //   double dy = it->pose.y - carrot_pose_it->pose.y;
+      //   distance_it += std::hypot(dx, dy);
+      //   if (distance_it > costmap_robot_->getCostmap()->getResolution())
+      //   {
+      //     prev_carrot_pose_it = it;
+      //     break;
+      //   }
+      // }
 
-      robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
-                                      ? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose)
-                                      : robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D());
+      // robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
+      //                                 ? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose)
+      //                                 : robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D());
 
-      robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
+      // robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
 
       // teb_local_planner::PoseSE2 start_pose(front);
       // teb_local_planner::PoseSE2 goal_pose(back);
       // const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec());
-      const double dir_path = 0.0;
+
+      
+      auto goal_pose = compute_plan_.poses.front().pose;
+      auto start_pose = pose.pose;
+      double angle_path = atan2(goal_pose.y - start_pose.y, goal_pose.x - start_pose.x);
+      double dir_path = cos(fabs(angle_path - goal_pose.theta));
       if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
         x_direction = dir_path > 0 ? FORWARD : BACKWARD;
+      else
+        x_direction = 0.0;
     }
     catch (std::exception &e)
     {