diff --git a/config/move_base_common_params.yaml b/config/move_base_common_params.yaml
index b9a0b0a..02f44d8 100755
--- a/config/move_base_common_params.yaml
+++ b/config/move_base_common_params.yaml
@@ -1,12 +1,12 @@
 
 ### replanning
-controller_frequency: 20.0   # run controller at 15.0 Hz
+controller_frequency: 30.0   # run controller at 15.0 Hz
 controller_patience: 0.0    # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan
 planner_frequency: 0.0       # don't continually replan (only when controller failed)
-planner_patience: 5.0        # if the first planning attempt failed, abort planning retries after 5.0 s...
+planner_patience: 2.0        # if the first planning attempt failed, abort planning retries after 5.0 s...
 max_planning_retries: 5     # ... or after 10 attempts (whichever happens first)
 oscillation_timeout: -1    # abort controller and trigger recovery behaviors after 30.0 s
-oscillation_distance: 0.4
+oscillation_distance: 0.5
 ### recovery behaviors
 recovery_behavior_enabled: true
 recovery_behaviors: [       
diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml
index 3a861f4..dbc8ca5 100644
--- a/config/pnkx_local_planner_params.yaml
+++ b/config/pnkx_local_planner_params.yaml
@@ -1,6 +1,6 @@
 yaw_goal_tolerance: 0.017  
-xy_goal_tolerance: 0.03 
-min_approach_linear_velocity: 0.06 
+xy_goal_tolerance: 0.02 
+min_approach_linear_velocity: 0.05 
 
 LocalPlannerAdapter:
   library_path: liblocal_planner_adapter
@@ -48,14 +48,14 @@ LimitedAccelGenerator:
   min_vel_y: 0.0  # diff drive robot
 
   max_speed_xy: 2.0    # max_trans_vel: 0.8  # choose slightly less than the base's capability
-  min_speed_xy: 0.1    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
+  min_speed_xy: 0.25    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
 
-  max_vel_theta: 0.9    # max_rot_vel: 1.0  # choose slightly less than the base's capability
-  min_vel_theta: 0.04  # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity
+  max_vel_theta: 0.7   # max_rot_vel: 1.0  # choose slightly less than the base's capability
+  min_vel_theta: 0.07  # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity
 
   acc_lim_x: 1.0
   acc_lim_y: 0.0      # diff drive robot
-  acc_lim_theta: 0.9
+  acc_lim_theta: 1.5
   decel_lim_x: -1.0
   decel_lim_y: -0.0
   decel_lim_theta: -1.5
@@ -75,8 +75,8 @@ LimitedAccelGenerator:
 MKTAlgorithmDiffPredictiveTrajectory:
   library_path: libmkt_algorithm_diff
   avoid_obstacles: false
-  xy_local_goal_tolerance: 0.01
-  angle_threshold: 0.6
+  xy_local_goal_tolerance: 0.02
+  angle_threshold: 0.47
   index_samples: 60
   follow_step_path: true
 
@@ -85,12 +85,12 @@ MKTAlgorithmDiffPredictiveTrajectory:
   # only when false:
   lookahead_dist: 0.5                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
   # only when true:
-  min_lookahead_dist: 0.5                     # The minimum lookahead distance (m) threshold. (default: 0.3)
+  min_lookahead_dist: 0.6                     # The minimum lookahead distance (m) threshold. (default: 0.3)
   max_lookahead_dist: 2.0                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
   lookahead_time: 1.9                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
-  min_journey_squared: 0.3                    # Minimum squared journey to consider for goal (default: 0.2) 
+  min_journey_squared: 0.2                    # Minimum squared journey to consider for goal (default: 0.2) 
   max_journey_squared: 0.8                    # Maximum squared journey to consider for goal (default: 0.2) 
-  max_lateral_accel: 2.0                      # Max lateral accel for speed reduction on curves (m/s^2)
+  max_lateral_accel: 0.9                      # Max lateral accel for speed reduction on curves (m/s^2)
 
   # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
   use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
@@ -100,7 +100,7 @@ MKTAlgorithmDiffPredictiveTrajectory:
 
   # stoped
   rot_stopped_velocity: 0.05
-  trans_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.06
   
   # Regulated linear velocity scaling
   use_regulated_linear_velocity_scaling: false   # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
@@ -114,20 +114,6 @@ MKTAlgorithmDiffPredictiveTrajectory:
   cost_scaling_dist: 0.2                             # (default: 0.6)
   cost_scaling_gain: 2.0                             # (default: 1.0)
 
-
-  use_mpc: true
-  mpc_horizon: 10
-  mpc_dt: 0.1
-  mpc_w_pos: 6.0
-  mpc_w_theta: 2.0
-  mpc_w_v: 0.2
-  mpc_w_w: 0.2
-  mpc_w_dv: 0.4
-  mpc_w_dw: 0.4
-  mpc_iterations: 3
-  mpc_step: 0.15
-  mpc_eps: 0.001
-
 MKTAlgorithmDiffGoStraight:
   library_path: libmkt_algorithm_diff
   avoid_obstacles: false
diff --git a/src/Algorithms/Cores/score_algorithm/README.md b/src/Algorithms/Cores/score_algorithm/README.md
new file mode 100644
index 0000000..b86fc66
--- /dev/null
+++ b/src/Algorithms/Cores/score_algorithm/README.md
@@ -0,0 +1,44 @@
+# score_algorithm
+
+## Giới thiệu
+`score_algorithm` là lớp nền (base class) cho các thuật toán đánh giá và sinh lệnh điều khiển (local planning) trong `pnkx_nav_core`. Module này không tự sinh quỹ đạo cụ thể, mà cung cấp bộ khung và các hàm dùng chung để các thuật toán con triển khai.
+
+## Vai trò chính
+- Chuẩn hóa giao diện (interface) cho các thuật toán tính toán quỹ đạo/lệnh.
+- Cắt đoạn `global_plan` thành đoạn con phù hợp với `local costmap`.
+- Chọn `sub-goal` và `start` để theo đuổi theo từng bước đường đi.
+- Cung cấp các hàm tiện ích: kiểm tra dừng/đi, tính quãng đường, tìm mục tiêu theo góc và khoảng cách.
+
+## API chính
+Được khai báo trong `include/score_algorithm/score_algorithm.h`:
+- `initialize(...)`: khởi tạo tham số, TF, costmap, bộ sinh quỹ đạo.
+- `prepare(...)`: chuẩn bị dữ liệu dùng chung trước khi đánh giá.
+- `calculator(...)`: tính toán ra `mkt_msgs::Trajectory2D`.
+- `computePlanCommand(...)`: cắt `global_plan` thành đoạn kết quả và chọn `start/goal`.
+- `findSubGoalIndex(...)`, `getGoalIndex(...)`, `journey(...)`, `stopped(...)`: hàm phụ trợ.
+
+## Nguyên lý `computePlanCommand(...)`
+Hàm này nhận `robot_pose`, `velocity`, độ dài xem trước `S` và `global_plan`, sau đó:
+1) Tìm các mốc `sub-goal` (đổi hướng / điểm gãy) bằng `findSubGoalIndex`.
+2) Khôi phục `sub_goal_index` và `sub_start_index` đã lưu (nếu có), cập nhật nếu robot đang dừng và đã vượt mốc.
+3) Tìm `start_index` gần nhất với robot trong đoạn con hợp lệ.
+4) Tìm `goal_index` dựa trên quy tắc "đi đủ xa `S` hoặc đổi hướng vượt ngưỡng".
+5) Xuất `result` là đoạn từ `closet_index` đến `goal_index`.
+
+Kết quả được sử dụng bởi các local planner để sinh lệnh điều khiển tiếp theo.
+
+## Sử dụng trong hệ thống
+`score_algorithm` được nạp qua pluginlib trong các local planner, ví dụ:
+- `pnkx_local_planner`
+- `pnkx_go_straight_local_planner`
+- `pnkx_rotate_local_planner`
+- `pnkx_docking_local_planner`
+
+Mỗi thuật toán cụ thể sẽ kế thừa `ScoreAlgorithm` và triển khai `initialize`, `prepare`, `calculator`.
+
+## Thư mục liên quan
+- `include/score_algorithm/score_algorithm.h`: giao diện chính.
+- `src/score_algorithm.cpp`: các hàm dùng chung (getGoalIndex, computePlanCommand, ...).
+
+## Ghi chú
+Module này phụ thuộc vào `robot_nav_2d_msgs`, `robot_costmap_2d`, `mkt_msgs` và hệ thống TF/ROS.
diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h
index e70c7ad..1e6ebd6 100755
--- a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h
+++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h
@@ -1,6 +1,8 @@
 #ifndef _SCORE_ALGORITHM_H_INCLUDED__
 #define _SCORE_ALGORITHM_H_INCLUDED__
 
+#include <ros/ros.h>
+#include <geometry_msgs/PoseStamped.h>
 #include <robot/robot.h>
 #include <iostream>
 #include <robot_nav_2d_msgs/Twist2DStamped.h>
@@ -153,11 +155,19 @@ namespace score_algorithm
     double old_xy_goal_tolerance_;
     double angle_threshold_;
 
+    ros::Publisher current_goal_pub_;
+    ros::Publisher closet_robot_goal_pub_;
 
     std::vector<robot_nav_2d_msgs::Pose2DStamped> reached_intermediate_goals_;
     std::vector<unsigned int> start_index_saved_vt_;
     unsigned int sub_goal_index_saved_, sub_goal_seq_saved_;
     unsigned int sub_start_index_saved_, sub_start_seq_saved_;
+
+    double max_vel_x_, min_vel_x_, acc_lim_x_, decel_lim_x_;
+    double max_vel_y_, min_vel_y_, acc_lim_y_, decel_lim_y_;
+    double max_vel_theta_, min_vel_theta_, acc_lim_theta_, decel_lim_theta_;
+    double min_speed_xy_, max_speed_xy_;
+    double rot_stopped_velocity_, trans_stopped_velocity_;
   };
 } // namespace score_algorithm
 
diff --git a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp
index 62e6851..3e5362b 100644
--- a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp
+++ b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp
@@ -1,3 +1,4 @@
+#include <robot/robot.h>
 #include <score_algorithm/score_algorithm.h>
 #include <robot_nav_2d_utils/path_ops.h>
 #include <robot_nav_2d_utils/conversions.h>
@@ -137,7 +138,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
   result.poses.clear();
   if (global_plan.poses.empty())
   {
-     std::cerr << "ScoreAlgorithm: The global plan was empty." << std::endl; 
+    robot::log_error("ScoreAlgorithm: The global plan was empty.");
     return false;
   }
 
@@ -145,7 +146,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
   std::vector<robot_nav_2d_msgs::Pose2DStamped> mutes;
   if (!this->findSubGoalIndex(global_plan.poses, seq_s, mutes))
   {
-    std::cerr << "ScoreAlgorithm: Cannot find SubGoal Index" << std::endl;
+    robot::log_error("ScoreAlgorithm: Cannot find SubGoal Index");
     return false;
   }
 
@@ -187,21 +188,19 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
     }
   }
 
-  // Handle empty seq_s
   if (index_s.empty())
   {
     sub_goal_index = (unsigned int)global_plan.poses.size();
     sub_goal_seq_saved_ = global_plan.poses.back().header.seq;
-    std::cout << "ScoreAlgorithm: seq_s is empty, setting sub_goal_index to " << sub_goal_index << std::endl;
   }
   else
   {
-    // Update sub_goal_index if index_s.front() is greater and valid
-    if (index_s.front() > sub_goal_index_saved_ && index_s.front() < (unsigned int)global_plan.poses.size())
+    bool is_stopped = stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_);
+    if (is_stopped && index_s.front() > sub_goal_index_saved_ && index_s.front() < (unsigned int)global_plan.poses.size())
     {
-      sub_goal_index = (unsigned int)index_s.front() > 0 ? (unsigned int)index_s.front() - 1 : (unsigned int)index_s.front();
+      // sub_goal_index = (unsigned int)index_s.front() > 0 ? (unsigned int)index_s.front() - 1 : (unsigned int)index_s.front();
+      sub_goal_index = index_s.front();
       sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
-      // std::cout << "ScoreAlgorithm: Updated sub_goal_index to " << sub_goal_index << " based on index_s.front()" << std::endl;
     }
 
     // Process index_s with multiple elements
@@ -211,7 +210,6 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
       {
         if (index_s[i - 1] >= (unsigned int)global_plan.poses.size() || index_s[i] >= (unsigned int)global_plan.poses.size())
         {
-          std::cout << "ScoreAlgorithm: Invalid index index_s[" << i - 1 << "]=" << index_s[i - 1] << " or index_s[" << i << "]=" << index_s[i] << ", plan size=" << (unsigned int)global_plan.poses.size() << std::endl;
           continue;
         }
 
@@ -226,12 +224,8 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
           double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
           if (fabs(tolerance) <= xy_local_goal_tolerance_)
           {
-            // ROS_INFO_THROTTLE(0.1, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), robot_pose.pose.theta, global_plan.poses[index_s[i - 1]].pose.theta);
-            // ROS_INFO_THROTTLE(0.1, "tolerance 1 %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
             if (index_s[i] > sub_goal_index_saved_)
             {
-              // ROS_INFO("%d %d %d %d %d", index_s[i], sub_goal_index, sub_goal_index_saved_, sub_goal_seq_saved_, (unsigned int)global_plan.poses.size());
-              // ROS_INFO_NAMED("ScoreAlgorithm", "Following from %u to %d", sub_goal_index, i < (unsigned int)(index_s.size() - 1) ? index_s[i] + 1 : (unsigned int)global_plan.poses.size());
               sub_goal_index = (i < index_s.size() - 1) ? index_s[i] : (unsigned int)global_plan.poses.size() - 1;
               sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
               old_xy_goal_tolerance_ = 0.0;
@@ -255,7 +249,6 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
       {
         sub_goal_index = (unsigned int)global_plan.poses.size() - 1;
         sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
-        // std::cout << "ScoreAlgorithm: Invalid sub_goal_index, setting to " << sub_goal_index << std::endl;
       }
       else
       {
@@ -269,12 +262,10 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
           double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
           if (fabs(tolerance) <= xy_local_goal_tolerance_)
           {
-            // ROS_INFO_THROTTLE(0.1, "%f %f %f %f ", fabs(cos(theta)), fabs(sin(theta)), robot_pose.pose.theta, global_plan.poses[sub_goal_index].pose.theta);
-            // ROS_INFO_THROTTLE(0.1, "tolerance 2 %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
             sub_goal_index = (unsigned int)global_plan.poses.size() - 1;
             sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
             old_xy_goal_tolerance_ = 0.0;
-            std::cout << "ScoreAlgorithm: Single sub-goal reached, setting sub_goal_index to " << sub_goal_index << std::endl;
+            robot::log_info("ScoreAlgorithm: Single sub-goal reached, setting sub_goal_index to %d", sub_goal_index);
           }
         }
         else
@@ -284,7 +275,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
   }
   if (sub_start_index >= sub_goal_index)
   {
-    std::cerr << "ScoreAlgorithm: Sub path is empty" << std::endl;
+    robot::log_error("ScoreAlgorithm: Sub path is empty");
     return false;
   }
 
@@ -310,7 +301,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
   // Nếu khồng tìm được điểm gần với robot nhất thì trả về false
   if (!started_path)
   {
-    std::cerr << "ScoreAlgorithm: None of the points of the global plan were in the local costmap." << std::endl;
+    robot::log_error("ScoreAlgorithm: None of the points of the global plan were in the local costmap.");
     return false;
   }
   std::stringstream ss;
@@ -369,17 +360,9 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
     // check if goal already reached
     bool goal_already_reached = false;
 
-    // robot_geometry_msgs::PoseArray poseArrayMsg;
-    // poseArrayMsg.header.stamp = robot::Time::now();
-    // poseArrayMsg.header.frame_id = costmap_robot_->getGlobalFrameID();
-    // int c = 0;
     for (auto &reached_intermediate_goal : reached_intermediate_goals_)
     {
       double distance = fabs(robot_nav_2d_utils::poseDistance(reached_intermediate_goal.pose, global_plan.poses[goal_index].pose));
-
-      // robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(reached_intermediate_goal.pose);
-      // poseArrayMsg.poses.push_back(pose);
-
       if (distance < xy_local_goal_tolerance_)
       {
         goal_already_reached = true;
@@ -403,7 +386,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
       {
         // the robot has currently reached the goal
         reached_intermediate_goals_.push_back(global_plan.poses[goal_index]);
-        std::cout << "ScoreAlgorithm: Number of reached_intermediate goals: " << reached_intermediate_goals_.size() << std::endl;
+        robot::log_info("ScoreAlgorithm: Number of reached_intermediate goals: %d", reached_intermediate_goals_.size());
       }
       else
       {
@@ -432,8 +415,37 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
   robot_nav_2d_msgs::Pose2DStamped sub_pose;
   sub_pose = global_plan.poses[closet_index];
 
+  {
+    robot_geometry_msgs::PoseStamped sub_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_pose);
+    geometry_msgs::PoseStamped sub_pose_stamped_ros;
+    sub_pose_stamped_ros.header.stamp = ros::Time::now();
+    sub_pose_stamped_ros.header.frame_id = sub_pose_stamped.header.frame_id;
+    sub_pose_stamped_ros.pose.position.x = sub_pose_stamped.pose.position.x;
+    sub_pose_stamped_ros.pose.position.y = sub_pose_stamped.pose.position.y;
+    sub_pose_stamped_ros.pose.position.z = sub_pose_stamped.pose.position.z;
+    sub_pose_stamped_ros.pose.orientation.x = sub_pose_stamped.pose.orientation.x;
+    sub_pose_stamped_ros.pose.orientation.y = sub_pose_stamped.pose.orientation.y;
+    sub_pose_stamped_ros.pose.orientation.z = sub_pose_stamped.pose.orientation.z;
+    sub_pose_stamped_ros.pose.orientation.w = sub_pose_stamped.pose.orientation.w;
+    closet_robot_goal_pub_.publish(sub_pose_stamped_ros);
+  }
+
   robot_nav_2d_msgs::Pose2DStamped sub_goal;
   sub_goal = global_plan.poses[goal_index];
+  {
+    robot_geometry_msgs::PoseStamped sub_goal_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_goal);
+    geometry_msgs::PoseStamped sub_goal_stamped_ros;
+    sub_goal_stamped_ros.header.stamp = ros::Time::now();
+    sub_goal_stamped_ros.header.frame_id = sub_goal_stamped.header.frame_id;
+    sub_goal_stamped_ros.pose.position.x = sub_goal_stamped.pose.position.x;
+    sub_goal_stamped_ros.pose.position.y = sub_goal_stamped.pose.position.y;
+    sub_goal_stamped_ros.pose.position.z = sub_goal_stamped.pose.position.z;
+    sub_goal_stamped_ros.pose.orientation.x = sub_goal_stamped.pose.orientation.x;
+    sub_goal_stamped_ros.pose.orientation.y = sub_goal_stamped.pose.orientation.y;
+    sub_goal_stamped_ros.pose.orientation.z = sub_goal_stamped.pose.orientation.z;
+    sub_goal_stamped_ros.pose.orientation.w = sub_goal_stamped.pose.orientation.w;
+    current_goal_pub_.publish(sub_goal_stamped_ros);
+  }
 
   result.header = global_plan.header;
   for (int i = closet_index; i <= goal_index; i++)
diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
index f4f8ac8..66857c8 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
+++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
@@ -144,16 +144,65 @@ namespace mkt_algorithm
           const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
           robot_nav_2d_msgs::Path2D &transformed_plan);
 
-      robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose);
+      /**
+       * @brief Calculate max kappa
+       * @param path
+       * @param preview_distance
+       * @return max kappa
+       */
+      double calculateMaxKappa(const robot_nav_2d_msgs::Path2D &path, const double preview_distance = std::numeric_limits<double>::max());
+    
+      /**
+       * @brief Adjust speed with Hermite trajectory
+       * @param velocity
+       * @param trajectory
+       * @param v_target
+       * @param sign_x
+       * @return speed
+       */
+      double adjustSpeedWithHermiteTrajectory(
+        const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Path2D &trajectory, double v_target, const double &sign_x
+      );
+
+      /**
+       * @brief Generate Hermite trajectory
+       * @param path
+       * @param drive_target
+       * @param velocity
+       * @param sign_x
+       * @param drive_cmd
+      * @return trajectory
+       */
+      robot_nav_2d_msgs::Path2D generateTrajectory(  
+        const robot_nav_2d_msgs::Path2D &path, const robot_nav_2d_msgs::Twist2D &drive_target,
+        const robot_nav_2d_msgs::Twist2D &velocity, const double &sign_x, robot_nav_2d_msgs::Twist2D &drive_cmd);
+
+      /**
+       * @brief Generate Hermite trajectory
+       * @param pose
+       * @param sign_x
+       * @return trajectory
+       */
+      robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x);
+
+      /**
+       * @brief Generate Hermite quadratic trajectory
+       * @param pose
+       * @param sign_x
+       * @return trajectory
+       */
+      robot_nav_2d_msgs::Path2D generateHermiteQuadraticTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x);
 
       /**
        * @brief Should rotate to path
        * @param carrot_pose
+       * @param velocity
+       * @param sign_x
        * @param angle_to_path
        * @return true if should rotate, false otherwise
        */
       bool shouldRotateToPath(
-          const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, const robot_nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x);
+          const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, robot_nav_2d_msgs::Twist2D velocity, double &angle_to_path, const double &sign_x);
 
       /**
        * @brief Rotate to heading
@@ -163,6 +212,40 @@ namespace mkt_algorithm
        */
       void rotateToHeading(const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel);
 
+      /**
+       * @brief Check if robot should align to final heading (near goal position)
+       * @param trajectory The transformed trajectory in robot frame
+       * @param goal The goal pose with desired final heading
+       * @param velocity Current velocity
+       * @param xy_error Output: distance to goal
+       * @param heading_error Output: angle difference to goal heading
+       * @return true if should enter final heading alignment phase
+       */
+      bool shouldAlignToFinalHeading(
+          const robot_nav_2d_msgs::Path2D &trajectory,
+          const robot_nav_2d_msgs::Pose2DStamped &goal,
+          const robot_nav_2d_msgs::Twist2D &velocity,
+          double &xy_error,
+          double &heading_error);
+
+      /**
+       * @brief Arc Motion controller for final heading alignment
+       * Smoothly reduces linear velocity while adjusting angular velocity to reach final heading
+       * @param xy_error Distance to goal position
+       * @param heading_error Angle difference to goal heading
+       * @param velocity Current velocity
+       * @param sign_x Direction of motion (forward/backward)
+       * @param dt Time step
+       * @param cmd_vel Output velocity command
+       */
+      void alignToFinalHeading(
+          const double &xy_error,
+          const double &heading_error,
+          const robot_nav_2d_msgs::Twist2D &velocity,
+          const double &sign_x,
+          const double &dt,
+          robot_nav_2d_msgs::Twist2D &cmd_vel);
+
       /**
        * @brief  the robot is moving acceleration limits
        * @param  velocity The velocity of the robot
@@ -182,20 +265,16 @@ namespace mkt_algorithm
       bool stopWithAccLimits(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel);
 
       /**
-       * @brief Apply constraints
-       * @param dist_error
-       * @param lookahead_dist
-       * @param curvature
-       * @param curr_speed
-       * @param pose_cost
-       * @param linear_vel
-       * @param sign
+       * @brief Compute pure pursuit
+       * @param carrot_pose
+       * @param drive_target
+       * @param velocity
+       * @param trajectory
+       * @param min_approach_linear_velocity
+       * @param sign_x
+       * @param dt
+       * @param drive_cmd
        */
-      void applyConstraints(
-          const double &dist_error, const double &lookahead_dist,
-          const double &curvature, const robot_nav_2d_msgs::Twist2D &curr_speed,
-          const double &pose_cost, double &linear_vel, const double &sign_x);
-
       void computePurePursuit(
         const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
         const robot_nav_2d_msgs::Twist2D &drive_target,
@@ -206,14 +285,15 @@ namespace mkt_algorithm
         const double &dt,
         robot_nav_2d_msgs::Twist2D &drive_cmd);
 
-      double adjustSpeedWithHermiteTrajectory(
-        const robot_nav_2d_msgs::Twist2D &velocity,
-        const robot_nav_2d_msgs::Path2D &trajectory,
-        double v_target,
-        const double &sign_x
-      );
-
-      std::vector<robot_geometry_msgs::Point> interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution);
+      /**
+       * @brief Interpolate footprint
+       * @param pose
+       * @param footprint
+       * @param resolution
+       * @return interpolated footprint
+       */
+      std::vector<robot_geometry_msgs::Point> interpolateFootprint(
+        const robot_geometry_msgs::Pose2D &pose, const std::vector<robot_geometry_msgs::Point> &footprint, const double &resolution);
 
       /**
        * @brief Cost at pose
@@ -223,11 +303,6 @@ namespace mkt_algorithm
        */
       double costAtPose(const double &x, const double &y);
 
-      double computeDecelerationFactor(double remaining_distance, double decel_distance);
-
-      double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, double journey_plan);
-
-      double estimateGoalHeading(const robot_nav_2d_msgs::Path2D &plan);
 
       bool initialized_;
       bool nav_stop_;
@@ -258,17 +333,17 @@ namespace mkt_algorithm
       // Rotate to heading
       bool use_rotate_to_heading_;
       double rotate_to_heading_min_angle_;
-
-      double max_vel_theta_, min_vel_theta_, acc_lim_theta_, decel_lim_theta_;
       double min_path_distance_, max_path_distance_;
 
+      // Final heading alignment (Arc Motion)
+      bool use_final_heading_alignment_;
+      double final_heading_xy_tolerance_;      // Distance threshold to trigger alignment
+      double final_heading_angle_tolerance_;   // Angle threshold to consider heading reached
+      double final_heading_min_velocity_;      // Minimum linear velocity during alignment
+      double final_heading_kp_angular_;        // Proportional gain for angular control
+
       // Regulated linear velocity scaling
       bool use_regulated_linear_velocity_scaling_;
-      double max_vel_x_, min_vel_x_, acc_lim_x_, decel_lim_x_;
-      double max_vel_y_, min_vel_y_, acc_lim_y_, decel_lim_y_;
-      double min_speed_xy_, max_speed_xy_;
-
-      double rot_stopped_velocity_, trans_stopped_velocity_;
 
       double min_approach_linear_velocity_;
       double regulated_linear_scaling_min_radius_;
@@ -291,6 +366,8 @@ namespace mkt_algorithm
       Eigen::MatrixXd R;
       Eigen::MatrixXd P;
 
+      ros::Publisher lookahead_point_pub_;
+
     }; // class PredictiveTrajectory
 
   } // namespace diff
diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.md b/src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.md
index 59665b6..9d58754 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.md
+++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.md
@@ -30,16 +30,21 @@ h11 =  t^3 - t^2
 ```
 
 ### Đạo hàm đầu/cuối
-Chọn độ dài đặc trưng `L` (thường L = sqrt(x^2 + y^2)):
+Chọn độ dài đặc trưng `L` (thường L = sqrt(x^2 + y^2)) và hướng `dir`:
 ```
-p'(0) = (L, 0)
-p'(1) = (L*cos(theta), L*sin(theta))
+dir = +1  (đi xuôi)
+dir = -1  (đi ngược 180° ở cả đầu và cuối)
+```
+Khi đó:
+```
+p'(0) = (dir * L, 0)
+p'(1) = (dir * L*cos(theta), dir * L*sin(theta))
 ```
 
 ### Công thức quỹ đạo
 ```
-x(t) = h10*L + h01*x + h11*L*cos(theta)
-y(t) = h01*y + h11*L*sin(theta)
+x(t) = h10*(dir*L) + h01*x + h11*(dir*L*cos(theta))
+y(t) = h01*y + h11*(dir*L*sin(theta))
 ```
 
 ### Hướng (heading) trên đường cong
@@ -50,8 +55,8 @@ h10' = 3t^2 - 4t + 1
 h01' = -6t^2 + 6t
 h11' = 3t^2 - 2t
 
-dx = h10'*L + h01'*x + h11'*L*cos(theta)
-dy = h01'*y + h11'*L*sin(theta)
+dx = h10'*(dir*L) + h01'*x + h11'*(dir*L*cos(theta))
+dy = h01'*y + h11'*(dir*L*sin(theta))
 ```
 Hướng:
 ```
diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.pdf b/src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.pdf
new file mode 100644
index 0000000..aabf3d7
Binary files /dev/null and b/src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.pdf differ
diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp
index a655108..bdd3d18 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp
+++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp
@@ -104,12 +104,9 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
 
   robot_nav_2d_msgs::Twist2D twist;
   traj_->startNewIteration(velocity); // Nhận tốc độ hiện tại từ odom
-  robot::Rate r(50);
   while (robot::ok() && traj_->hasMoreTwists())
   {
     twist = traj_->nextTwist();
-    // ROS_INFO("Twist: x: %.2f, y: %.2f, theta: %.2f", twist.x, twist.y, twist.theta);
-    r.sleep();
   }
   
   drive_cmd.x = sqrt(twist.x * twist.x);
@@ -120,105 +117,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
     return result;
   }
 
-  // Dynamically adjust look ahead distance based on the speed
-  double lookahead_dist = this->getLookAheadDistance(twist);
-  // Return false if the transformed global plan is empty
-  robot_geometry_msgs::Pose2D front = transform_plan_.poses.size() > 3 ? (*(transform_plan_.poses.begin() + 1)).pose : transform_plan_.poses.front().pose;
-  lookahead_dist += fabs(front.y);
 
-  // Get lookahead point and publish for visualization
-  auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
-
-  // Carrot distance squared
-  const double carrot_dist2 =
-      (carrot_pose.pose.x * carrot_pose.pose.x) +
-      (carrot_pose.pose.y * carrot_pose.pose.y);
-
-  // Find curvature of circle (k = 1 / R)
-  double curvature = 0.0;
-  if (carrot_dist2 > 2e-2)
-  {
-    curvature = 2.0 * carrot_pose.pose.y / carrot_dist2;
-  }
-
-  // Constrain linear velocity
-  this->applyConstraints(
-      0.0, lookahead_dist, curvature, twist,
-      this->costAtPose(pose.pose.x, pose.pose.y),
-      drive_cmd.x, sign_x);
-
-
-  if (std::hypot(carrot_pose.pose.x, carrot_pose.pose.y) > min_journey_squared_)
-  {
-    robot_nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back();
-    robot_nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2];
-    double dx = end.pose.x - start.pose.x;
-    double dy = end.pose.y - start.pose.y;
-    drive_cmd.theta = atan2(dy, dx);
-
-    //[-π/2, π/2]
-    if (drive_cmd.theta > M_PI / 2)
-      drive_cmd.theta -= M_PI;
-    else if (drive_cmd.theta < -M_PI / 2)
-      drive_cmd.theta += M_PI;
-
-    drive_cmd.theta = std::clamp(drive_cmd.theta, -min_vel_theta_, min_vel_theta_);
-  }
-  else
-    drive_cmd.theta = 0.0;
-
-  // populate and return message
-  if (fabs(carrot_pose.pose.x) > 1e-9 || carrot_pose.pose.y > 1e-9)
-    drive_cmd.x *= fabs(cos(std::atan2(carrot_pose.pose.y, carrot_pose.pose.x)));
-    
-  double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
-  const double vel_x_reduce = std::min(fabs(v_max), 0.3);
-  double journey_plan = compute_plan_.poses.empty() ? 0 : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1);
-  const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
-  const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale;
-  double d_reduce = std::clamp(journey_plan, min_S, max_S);
-  double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0);
-  double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce)));
-  double v_min = 
-    journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_;
-  v_min *= sign_x;
-  
-  double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
-  double decel_factor = computeDecelerationFactor(effective_journey, d_reduce);
-  journey_plan += max_path_distance_;
-  drive_cmd.x = journey_plan >= d_reduce ? drive_cmd.x : (drive_cmd.x - v_min) * decel_factor + v_min;
-
-  // drive_cmd.x = journey_plan > d_reduce ? drive_cmd.x : (drive_cmd.x - v_min) * sin((journey_plan / d_reduce) * (M_PI / 2)) + v_min;
-
-  Eigen::VectorXd y(2);
-  y << drive_cmd.x, drive_cmd.theta;
-
-  // Cập nhật lại A nếu dt thay đổi
-  for (int i = 0; i < n_; ++i)
-    for (int j = 0; j < n_; ++j)
-      A(i, j) = (i == j ? 1.0 : 0.0);
-  for (int i = 0; i < n_; i += 3)
-  {
-    A(i, i + 1) = dt;
-    A(i, i + 2) = 0.5 * dt * dt;
-    A(i + 1, i + 2) = dt;
-  }
-  kf_->update(y, dt, A);
-
-  if (drive_cmd.x != v_max)
-  {
-    double min = drive_cmd.x < v_max ? drive_cmd.x : v_max;
-    double max = drive_cmd.x > v_max ? drive_cmd.x : v_max;
-    drive_cmd.x = std::clamp(kf_->state()[0], min, max);
-  }
-  else
-  {
-    drive_cmd.x = std::clamp(kf_->state()[0], v_max, v_max);
-  }
-
-  drive_cmd.x = fabs(drive_cmd.x) >= min_approach_linear_velocity_ ? drive_cmd.x : std::copysign(min_approach_linear_velocity_, sign_x);
-  drive_cmd.theta = kf_->state()[3];
-  result.velocity = drive_cmd;
   return result;
 }
 
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 fd47170..96046fc 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
@@ -1,4 +1,4 @@
-#include <mkt_algorithm/diff/diff_predictive_trajectory_.h>
+#include <mkt_algorithm/diff/diff_predictive_trajectory.h>
 #include <boost/dll/alias.hpp>
 
 mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() {}
@@ -77,7 +77,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
     x0 << 0, 0, 0, 0, 0, 0;
     kf_->init(robot::Time::now().toSec(), x0);
 
-
+    ros::NodeHandle nh_ros;
+    current_goal_pub_ = nh_ros.advertise<geometry_msgs::PoseStamped>("current_goal", 10);
+    closet_robot_goal_pub_ = nh_ros.advertise<geometry_msgs::PoseStamped>("closet_robot_goal", 10);
+    lookahead_point_pub_ = nh_ros.advertise<geometry_msgs::PoseStamped>("lookahead_point", 10);
 
     x_direction_ = y_direction_ = theta_direction_ = 0;
     this->initialized_ = true;
@@ -104,12 +107,19 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
   nh_priv_.param<bool>("use_rotate_to_heading", use_rotate_to_heading_, false);
   nh_priv_.param<double>("rotate_to_heading_min_angle", rotate_to_heading_min_angle_, 0.785);
 
+  // Final heading alignment (Arc Motion)
+  nh_priv_.param<bool>("use_final_heading_alignment", use_final_heading_alignment_, true);
+  nh_priv_.param<double>("final_heading_xy_tolerance", final_heading_xy_tolerance_, 0.15);
+  nh_priv_.param<double>("final_heading_angle_tolerance", final_heading_angle_tolerance_, 0.05);
+  nh_priv_.param<double>("final_heading_min_velocity", final_heading_min_velocity_, 0.05);
+  nh_priv_.param<double>("final_heading_kp_angular", final_heading_kp_angular_, 1.5);
+
   nh_priv_.param<double>("rot_stopped_velocity", rot_stopped_velocity_, 0.01);
   nh_priv_.param<double>("trans_stopped_velocity", trans_stopped_velocity_, 0.01);
   if (trans_stopped_velocity_ < 0.02)
     trans_stopped_velocity_ = 0.5 * min_approach_linear_velocity_;
   if (trans_stopped_velocity_ > min_approach_linear_velocity_)
-    min_approach_linear_velocity_ = trans_stopped_velocity_ * 2.0;
+    trans_stopped_velocity_ = min_approach_linear_velocity_ + 0.01;
 
   // Regulated linear velocity scaling
   nh_priv_.param<bool>("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false);
@@ -302,7 +312,8 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
   y_direction = y_direction_ = 0;
   theta_direction = theta_direction_;
 
-  if ((unsigned int)(compute_plan_.poses.size() > 1) && compute_plan_.poses.back().header.seq != global_plan_.poses.back().header.seq)
+  if ((unsigned int)(compute_plan_.poses.size() > 1) && 
+      journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1) >= costmap_robot_->getCostmap()->getResolution())
   {
     const robot_geometry_msgs::Pose2D p2 = compute_plan_.poses.back().pose;
     int index;
@@ -320,6 +331,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
       if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
         x_direction = dir_path > 0 ? FORWARD : BACKWARD;
     }
+    
   }
   else
   {
@@ -403,14 +415,28 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     prevous_drive_cmd_ = drive_cmd;
     return result;
   }
-
   double lookahead_dist = getLookAheadDistance(velocity);
   auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
+  robot_geometry_msgs::PoseStamped carrot_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(carrot_pose);
+  {
+    geometry_msgs::PoseStamped lookahead_point;
+    lookahead_point.header.stamp = ros::Time::now();
+    lookahead_point.header.frame_id = carrot_pose_stamped.header.frame_id;
+    lookahead_point.pose.position.x = carrot_pose_stamped.pose.position.x;
+    lookahead_point.pose.position.y = carrot_pose_stamped.pose.position.y;
+    lookahead_point.pose.position.z = carrot_pose_stamped.pose.position.z;
+    lookahead_point.pose.orientation.x = carrot_pose_stamped.pose.orientation.x;
+    lookahead_point.pose.orientation.y = carrot_pose_stamped.pose.orientation.y;
+    lookahead_point.pose.orientation.z = carrot_pose_stamped.pose.orientation.z;
+    lookahead_point.pose.orientation.w = carrot_pose_stamped.pose.orientation.w;
+    lookahead_point_pub_.publish(lookahead_point);
+  }
+
   bool allow_rotate = false;
   nh_priv_.param("allow_rotate", allow_rotate, false);
 
   const double distance_allow_rotate = min_journey_squared_;
-  const double path_distance_to_rotate = hypot(pose.pose.x - transformed_plan.poses.back().pose.x, pose.pose.y - transformed_plan.poses.back().pose.y);
+  const double path_distance_to_rotate = journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1);
   allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
 
   double angle_to_heading;
@@ -428,18 +454,27 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   }
   else
   {
-    double v_target = this->adjustSpeedWithHermiteTrajectory(velocity, transformed_plan, drive_cmd.x, sign_x);
-    robot_nav_2d_msgs::Twist2D drive_target;
-    drive_target.x = v_target;
-    transformed_plan = this->generateHermiteTrajectory(transformed_plan.poses.back());
     if(fabs(carrot_pose.pose.y) > 0.2)
     {
       lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
-    }  
+    }
+    robot_nav_2d_msgs::Twist2D drive_target;
+    transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
     carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
-    
-    const double journey_plan = transformed_plan.poses.empty() ? distance_allow_rotate : journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1);
-    this->computePurePursuit(
+
+    // === Final Heading Alignment Check ===
+    double xy_error = 0.0, heading_error = 0.0;
+    if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error))
+    {
+      // Use Arc Motion controller for final heading alignment
+      alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
+      ROS_INFO_THROTTLE(0.5, "[FinalHeadingAlign] xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w=%.3f",
+                        xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, drive_cmd.theta);
+    }
+    else
+    {
+      // Normal Pure Pursuit
+      this->computePurePursuit(
       carrot_pose, 
       drive_target,
       velocity, 
@@ -448,24 +483,43 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
       sign_x, 
       dt, 
       drive_cmd);
+    }
+    double s = compute_plan_.poses.empty() 
+      ? min_journey_squared_
+      : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1);
+    double S_th = 0.1 + max_path_distance_ + fabs(velocity.x) * lookahead_time_;  
+    double S_final = std::min(S_th * 0.2, 0.1 + max_path_distance_);
 
-    const double vel_x_reduce = 0.3;
-    const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
-    const double min_S = min_lookahead_dist_ + max_path_distance_, max_S = max_lookahead_dist_ + max_path_distance_;
-    double d_reduce = std::clamp(journey_plan, min_S, max_S);
-    double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0);
-    double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce)));
-    double v_min =
-        journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_;
-    v_min *= sign_x;
+    double max_speed = fabs(drive_cmd.x);
+    double target_speed = max_speed;
+    if (s < S_th)
+    {
+      double r = std::clamp(s / S_th, 0.0, 1.0);
+      double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r)));
+      target_speed = max_speed * cosine_factor;
+    }
 
-    double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
-    double decel_factor = computeDecelerationFactor(effective_journey, d_reduce);
-    double vel_reduce = sign_x > 0
-                            ? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min)
-                            : std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min);
-    drive_cmd.x = journey_plan  > d_reduce ? drive_cmd.x : vel_reduce;
-    robot::log_info("journey_plan: %f, max_path_distance_: %f, d_reduce: %f, vel_reduce: %f", journey_plan, max_path_distance_, d_reduce, vel_reduce);
+    double reduce_speed = std::min(max_speed, min_speed_xy_);
+    if (s < S_final)
+    {
+      double r = std::clamp(s / S_final, 0.0, 1.0);
+      double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r)));
+      reduce_speed = (reduce_speed - min_approach_linear_velocity_) * cosine_factor + min_approach_linear_velocity_;
+      reduce_speed = std::max(reduce_speed, min_approach_linear_velocity_);
+    }
+    target_speed = std::max(target_speed, reduce_speed);
+
+    double v_desired = std::copysign(target_speed, sign_x);
+    double max_acc_vel = velocity.x + fabs(acc_lim_x_) * dt;
+    double min_acc_vel = velocity.x - fabs(acc_lim_x_) * dt;
+    double v_samp = std::clamp(v_desired, min_acc_vel, max_acc_vel);
+
+    double max_speed_to_stop = std::sqrt(2.0 * fabs(decel_lim_x_) * std::max(0.0, s));
+    v_samp = std::copysign(std::min(max_speed_to_stop, fabs(v_samp)), sign_x);
+    if (sign_x > 0.0)
+      drive_cmd.x = std::clamp(v_samp, min_approach_linear_velocity_, max_speed);
+    else
+      drive_cmd.x = std::clamp(v_samp, -max_speed, -min_approach_linear_velocity_);
 
     if (this->nav_stop_)
     {
@@ -504,70 +558,18 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   result.poses.reserve(transformed_plan.poses.size());
   for (const auto &pose_stamped : transformed_plan.poses)
   {
+    result.poses.push_back(pose_stamped.pose);
     if(fabs(pose_stamped.pose.x - carrot_pose.pose.x) < 1e-6 && 
       fabs(pose_stamped.pose.y - carrot_pose.pose.y) < 1e-6 && 
       fabs(pose_stamped.pose.theta - carrot_pose.pose.theta) < 1e-6)
-      break;
-    result.poses.push_back(pose_stamped.pose);
+    break;
   }
+  
   result.velocity = drive_cmd;
   prevous_drive_cmd_ = drive_cmd;
   return result;
 }
 
-double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajectory(
-  const robot_nav_2d_msgs::Twist2D &velocity,
-  const robot_nav_2d_msgs::Path2D &trajectory,
-  double v_target,
-  const double &sign_x)
-{
-  if (journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
-    return min_speed_xy_ * sign_x;
-
-  // Use speed-scaled preview distance to evaluate curvature
-  double preview_dist = std::clamp(std::fabs(velocity.x) * lookahead_time_, min_lookahead_dist_, max_lookahead_dist_);
-
-  double traveled = 0.0;
-  double max_kappa = 0.0;
-  for (size_t i = 1; i + 1 < trajectory.poses.size(); ++i)
-  {
-    const auto &p0 = trajectory.poses[i - 1].pose;
-    const auto &p1 = trajectory.poses[i].pose;
-    const auto &p2 = trajectory.poses[i + 1].pose;
-
-    const double a = std::hypot(p1.x - p0.x, p1.y - p0.y);
-    const double b = std::hypot(p2.x - p1.x, p2.y - p1.y);
-    const double c = std::hypot(p2.x - p0.x, p2.y - p0.y);
-    traveled += a;
-
-    if (a > 1e-6 && b > 1e-6 && c > 1e-6)
-    {
-      const double cross = (p1.x - p0.x) * (p2.y - p0.y) - (p1.y - p0.y) * (p2.x - p0.x);
-      const double area2 = std::fabs(cross); // 2 * area
-      const double kappa = 2.0 * area2 / (a * b * c);
-      max_kappa = std::max(max_kappa, std::fabs(kappa));
-    }
-
-    if (traveled >= preview_dist)
-      break;
-  }
-  double v_limit = std::fabs(v_target);
-  if (max_kappa > 1e-6 && max_lateral_accel_ > 1e-6)
-  {
-    const double v_curve = std::sqrt(max_lateral_accel_ / max_kappa);
-    v_limit = std::min(v_limit, v_curve);
-  }
-
-  if (decel_lim_x_ > 1e-6)
-  {
-    const double remaining = journey(trajectory.poses, 0, trajectory.poses.size() - 1);
-    const double v_stop = std::sqrt(2.0 * decel_lim_x_ * std::max(0.0, remaining));
-    v_limit = std::min(v_limit, v_stop);
-  }
-
-  return std::copysign(v_limit, sign_x);
-}
-
 void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
   const robot_nav_2d_msgs::Twist2D &drive_target,
@@ -580,12 +582,17 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
 {
   // 1) Curvature from pure pursuit
   const double L2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
-  if (L2 < 1e-6)
-    return;
-  const double kappa = 2.0 * carrot_pose.pose.y / L2;
+  const double L2_min = 0.05;          // m^2, chỉnh theo nhu cầu (0.02–0.1)
+  const double L2_safe = std::max(L2, L2_min);
+  const double L = std::sqrt(L2_safe);
+
+  const double kappa = 2.0 * carrot_pose.pose.y / L2_safe;
 
   // 3) Adjust speed using Hermite trajectory curvature + remaining distance
   double v_target = adjustSpeedWithHermiteTrajectory(velocity, trajectory, drive_target.x, sign_x);
+  const double L_min = 0.1;  // m, chỉnh theo nhu cầu
+  double scale_close = std::clamp(L / L_min, 0.0, 1.0);
+  v_target *= scale_close;
 
   const double y_abs = std::fabs(carrot_pose.pose.y);
   const double y_soft = 0.1;
@@ -606,7 +613,25 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
 
   // 5) Angular speed from curvature
   double w_target = v_target * kappa;
-  w_target = std::clamp(w_target, -max_vel_theta_, max_vel_theta_);
+  if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
+  {
+    const double k_heading = 0.3;
+    if (trajectory.poses.size() >= 2) {
+      const auto& p1 = trajectory.poses[trajectory.poses.size() - 2].pose;
+      const auto& p2 = trajectory.poses[trajectory.poses.size() - 1].pose;
+      double heading_ref = std::atan2(p2.y - p1.y, p2.x - p1.x);
+    
+      // robot heading in base_link = 0, so error = heading_ref
+      double w_heading = k_heading * angles::normalize_angle(heading_ref);
+    
+      w_target += w_heading;
+    }
+    else
+    {
+      w_target = 0.0;
+    }
+  }
+  w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
 
   // 6) Apply acceleration limits (linear + angular)
   const double dv = std::clamp(v_target - velocity.x, -acc_lim_x_ * dt, acc_lim_x_ * dt);
@@ -616,151 +641,42 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   drive_cmd.theta = velocity.theta + dw;
 }
 
-double mkt_algorithm::diff::PredictiveTrajectory::estimateGoalHeading(const robot_nav_2d_msgs::Path2D &plan) {
-  if (plan.poses.size() < 2) return 0.0;
-  const auto& p1 = plan.poses[plan.poses.size() - 2];
-  const auto& p2 = plan.poses[plan.poses.size() - 1];
-  return std::atan2(p2.pose.y - p1.pose.y, p2.pose.x - p1.pose.x);
-}
-
-void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints(
-  const double &dist_error, const double &lookahead_dist,
-  const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity,
-  const double &pose_cost, double &linear_vel, const double &sign_x)
-{
-  double curvature_vel = linear_vel;
-  double cost_vel = linear_vel;
-  double approach_vel = linear_vel;
-
-
-  if (use_regulated_linear_velocity_scaling_)
-  {
-    const double &min_rad = regulated_linear_scaling_min_radius_;
-    const double radius = curvature > 1e-9 ? fabs(1.0 / curvature) : min_rad;
-    if (radius < min_rad)
-    {
-      curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad);
-      robot_nav_2d_msgs::Twist2D cmd, result;
-      cmd.x = curvature_vel;
-      this->moveWithAccLimits(velocity, cmd, result);
-      curvature_vel = result.x;
-      linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_);
-    }
-  }
-
-  if (use_cost_regulated_linear_velocity_scaling_ &&
-      pose_cost != static_cast<double>(robot_costmap_2d::NO_INFORMATION) &&
-      pose_cost != static_cast<double>(robot_costmap_2d::FREE_SPACE))
-  {
-    const double inscribed_radius = costmap_robot_->getLayeredCostmap()->getInscribedRadius();
-    const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) *
-                                                std::log(pose_cost / (robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) +
-                                            inscribed_radius;
-
-    if (min_distance_to_obstacle < cost_scaling_dist_)
-    {
-      cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_;
-    }
-    robot_nav_2d_msgs::Twist2D cmd, result;
-    cmd.x = cost_vel;
-    this->moveWithAccLimits(velocity, cmd, result);
-    cost_vel = result.x;
-    linear_vel = std::min(cost_vel, curvature_vel);
-  }
-  // ss << linear_vel << " ";
-  // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed
-  // if the actual lookahead distance is shorter than requested, that means we're at the
-  // end of the path. We'll scale linear velocity by error to slow to a smooth stop.
-  // This expression is eq. to
-  //      (1) holding time to goal, t, constant using the theoretical
-  //          lookahead distance and proposed velocity and
-  //      (2) using t with the actual lookahead
-  //          distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t).
-
-  double dist_error_limit = costmap_robot_ != nullptr && costmap_robot_->getCostmap() != nullptr
-                                ? 2.0 * costmap_robot_->getCostmap()->getResolution()
-                                : 0.1;
-  if (dist_error > dist_error_limit)
-  {
-    double velocity_scaling = lookahead_dist > 1e-9 ? 1.0 - (dist_error / lookahead_dist) : 1.0;
-    double unbounded_vel = approach_vel * velocity_scaling;
-    if (unbounded_vel < min_approach_linear_velocity_)
-    {
-      approach_vel = min_approach_linear_velocity_;
-    }
-    else
-    {
-      approach_vel *= velocity_scaling;
-    }
-
-    // Use the lowest velocity between approach and other constraints, if all overlapping
-    robot_nav_2d_msgs::Twist2D cmd, result;
-    cmd.x = approach_vel;
-    this->moveWithAccLimits(velocity, cmd, result);
-    approach_vel = result.x;
-    linear_vel = std::min(linear_vel, approach_vel);
-  }
-
-  // Limit linear velocities to be valid
-  double min_vel_x = traj_ ? min_vel_x_ : fabs(traj_->getTwistLinear(false).x);
-  double max_vel_x = traj_ ? max_vel_x_ : fabs(traj_->getTwistLinear(true).x);
-  double min_vel_y = traj_ ? min_vel_y_ : fabs(traj_->getTwistLinear(false).y);
-  double max_vel_y = traj_ ? max_vel_y_ : fabs(traj_->getTwistLinear(true).y);
-  double max_linear_vel = sqrt(max_vel_x * max_vel_x + max_vel_y * max_vel_y);
-  double min_linear_vel = sqrt(min_vel_x * min_vel_x + min_vel_y * min_vel_y);
-  double desired_linear_vel = sign_x > 0 ? fabs(max_linear_vel) : fabs(min_linear_vel);
-  linear_vel = std::clamp(fabs(linear_vel), min_approach_linear_velocity_, desired_linear_vel);
-  linear_vel = sign_x * linear_vel;
-}
-
-
 bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
-    const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, const robot_nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x)
+    const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, 
+    robot_nav_2d_msgs::Twist2D velocity, double &angle_to_path, const double &sign_x)
 {
   bool curvature = false;
-  double path_angle = 0;
-  if (!global_plan.poses.empty())
-  {
-    if ((unsigned)global_plan.poses.size() > 2)
-    {
-      const double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x;
-      const double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y;
-      if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9)
-      {
-        double start_angle = atan2(start_direction_y, start_direction_x);
-        for (unsigned int i = 1; i < (unsigned)global_plan.poses.size(); i++)
-        {
-          const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x;
-          const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y;
-          if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
-          {
-            double current_angle = atan2(current_direction_y, current_direction_x);
-            if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) > 1e-9)
-            {
-              curvature = true;
-              break;
-            }
-            path_angle = current_angle;
-          }
-        }
-      }
-    }
-  }
-  // Whether we should rotate robot to rough path heading
-  angle_to_path = curvature ? path_angle : std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
-  angle_to_path = sign_x < 0
-                      ? angles::normalize_angle(M_PI + angle_to_path)
-                      : angles::normalize_angle(angle_to_path);
+  bool is_stopped = stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_);
 
+  double max_kappa = calculateMaxKappa(global_plan);
+  const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05));
+  curvature = max_kappa > straight_threshold;
+  double path_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
+  // Whether we should rotate robot to rough path heading
+  double blend = 0.85;  // Tỉ lệ blend
+  angle_to_path = curvature ? blend * path_angle : path_angle;
+
+  angle_to_path = sign_x < 0.0 ? angles::normalize_angle(M_PI + angle_to_path) : angle_to_path;
   double heading_linear = sqrt(velocity.x * velocity.x + velocity.y * velocity.y);
   // The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
   double heading_rotate = rotate_to_heading_min_angle_;
-  bool is_stopped = stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_);
+  
   heading_rotate = is_stopped
                        ? rotate_to_heading_min_angle_ + heading_linear
-                       : std::clamp(heading_rotate + heading_linear, 0.3, M_PI_2);
+                       : std::clamp(heading_rotate + heading_linear, angle_threshold_ + rotate_to_heading_min_angle_, M_PI_2);
 
-  bool result = use_rotate_to_heading_ && (is_stopped || sign(angle_to_path) * sign_x < 0) && fabs(angle_to_path) > heading_rotate;
+ 
+  bool result = use_rotate_to_heading_ && 
+    (is_stopped || sign(angle_to_path) * sign_x < 0 ) && 
+    fabs(angle_to_path) > heading_rotate;
+
+  // if (result)
+  //   ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
+  // else if(fabs(velocity.x) < min_speed_xy_)
+  // {
+  //   ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta);
+  //   ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
+  // }
   return result;
 }
 
@@ -771,6 +687,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &an
   double vel_yaw = theta_direction * fabs(velocity.theta);
   double ang_diff = angle_to_path;
   double max_vel_theta = traj_ ? fabs(traj_->getTwistAngular(true).z) : 0.3;
+  double min_vel_theta = min_vel_theta_;
 
   double angular_decel_zone = 0.3; // rad - khoảng cách bắt đầu giảm tốc
   nh_priv_.param("angular_decel_zone", angular_decel_zone, 0.5);
@@ -779,8 +696,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &an
   double zone_begin = std::max(angular_decel_zone * 0.2, 0.2);
   double cosine_begin_factor = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(ang_diff) / zone_begin)));
 
-  double reduce_vel_theta = std::min(max_vel_theta, 0.25 + min_vel_theta_);
-  reduce_vel_theta = fabs(ang_diff) > zone_begin ? reduce_vel_theta : (reduce_vel_theta - min_vel_theta_) * cosine_begin_factor + min_vel_theta_;
+  double reduce_vel_theta = std::min(max_vel_theta, 0.25 + min_vel_theta);
+  reduce_vel_theta = fabs(ang_diff) > zone_begin ? reduce_vel_theta : (reduce_vel_theta - min_vel_theta) * cosine_begin_factor + min_vel_theta_;
 
   // === ANGULAR DECELERATION ZONE ===
   double target_angular_speed = max_vel_theta;
@@ -810,14 +727,161 @@ void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &an
 
   // === FINAL LIMITS ===
   v_theta_samp = theta_direction > 0
-                     ? std::clamp(v_theta_samp, min_vel_theta_, max_vel_theta)
-                     : std::clamp(v_theta_samp, -max_vel_theta, -min_vel_theta_);
+                     ? std::clamp(v_theta_samp, min_vel_theta, max_vel_theta)
+                     : std::clamp(v_theta_samp, -max_vel_theta, -min_vel_theta);
 
   cmd_vel.x = 0.0;
   cmd_vel.y = 0.0;
   cmd_vel.theta = v_theta_samp;
 }
 
+bool mkt_algorithm::diff::PredictiveTrajectory::shouldAlignToFinalHeading(
+    const robot_nav_2d_msgs::Path2D &trajectory,
+    const robot_nav_2d_msgs::Pose2DStamped &goal,
+    const robot_nav_2d_msgs::Twist2D &velocity,
+    double &xy_error,
+    double &heading_error)
+{
+  if (!use_final_heading_alignment_)
+    return false;
+
+  if (trajectory.poses.empty())
+    return false;
+
+  // Calculate distance to goal (last point in trajectory is closest to goal in robot frame)
+  const auto &last_pose = trajectory.poses.back().pose;
+  xy_error = std::hypot(last_pose.x, last_pose.y);
+
+  // Transform goal heading to robot frame
+  // In robot frame, robot heading is 0, so heading_error = goal_heading_in_robot_frame
+  robot_nav_2d_msgs::Pose2DStamped goal_robot_frame;
+  if (tf_ && robot_nav_2d_utils::transformPose(tf_, robot_base_frame_, goal, goal_robot_frame))
+  {
+    heading_error = angles::normalize_angle(goal_robot_frame.pose.theta);
+  }
+  else
+  {
+    // Fallback: use last pose heading in trajectory
+    heading_error = angles::normalize_angle(last_pose.theta);
+  }
+
+  // Check if we're close enough to goal position to start final heading alignment
+  double distance_threshold = final_heading_xy_tolerance_;
+
+  // Dynamic threshold: increase when moving fast to allow smooth transition
+  double velocity_factor = std::max(0.0, std::fabs(velocity.x) * 0.5);
+  distance_threshold += velocity_factor;
+
+  bool near_goal = xy_error < distance_threshold;
+  bool heading_not_reached = std::fabs(heading_error) > final_heading_angle_tolerance_;
+
+  return near_goal && heading_not_reached;
+}
+
+void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
+    const double &xy_error,
+    const double &heading_error,
+    const robot_nav_2d_msgs::Twist2D &velocity,
+    const double &sign_x,
+    const double &dt,
+    robot_nav_2d_msgs::Twist2D &cmd_vel)
+{
+  // ============================================================
+  // Arc Motion Controller for Final Heading Alignment
+  // ============================================================
+  //
+  // Strategy:
+  // 1. Linear velocity: proportional to xy_error, decreasing as we approach goal
+  // 2. Angular velocity: proportional to heading_error with feedforward
+  // 3. Coordinate both to arrive at goal with correct heading
+  //
+  // Key insight: For arc motion, the relationship is:
+  //   arc_length = radius * angle
+  //   v = omega * radius
+  //
+  // We want to cover xy_error distance while rotating heading_error angle
+  // So ideal radius = xy_error / |heading_error| (when heading_error != 0)
+  // ============================================================
+
+  const double abs_heading_error = std::fabs(heading_error);
+  const double heading_sign = (heading_error > 0) ? 1.0 : -1.0;
+
+  // --- Linear velocity calculation ---
+  // Base velocity proportional to distance, with minimum for smooth motion
+  double v_base = std::sqrt(2.0 * std::fabs(decel_lim_x_) * xy_error);
+  v_base = std::max(v_base, final_heading_min_velocity_);
+  v_base = std::min(v_base, min_speed_xy_);
+
+  // Scale down when heading error is large (prioritize rotation)
+  double heading_scale = 1.0;
+  if (abs_heading_error > 0.1)  // ~5.7 degrees
+  {
+    // Reduce linear velocity when heading error is significant
+    heading_scale = std::clamp(0.3 / abs_heading_error, 0.3, 1.0);
+  }
+
+  double v_target = v_base * heading_scale * sign_x;
+
+  // --- Angular velocity calculation ---
+  double omega_target = 0.0;
+
+  if (abs_heading_error > final_heading_angle_tolerance_)
+  {
+    // Method 1: Proportional control
+    double omega_p = final_heading_kp_angular_ * heading_error;
+
+    // Method 2: Arc-based feedforward (coordinate linear and angular motion)
+    double omega_arc = 0.0;
+    if (xy_error > 0.01 && abs_heading_error > 0.01)
+    {
+      // Ideal omega to complete rotation while traveling remaining distance
+      // omega = v * heading_error / xy_error
+      omega_arc = std::fabs(v_target) * heading_error / xy_error;
+    }
+
+    // Blend proportional and arc-based control
+    // Use more arc-based when far, more proportional when close
+    double blend_factor = std::clamp(xy_error / final_heading_xy_tolerance_, 0.0, 1.0);
+    omega_target = blend_factor * omega_arc + (1.0 - blend_factor) * omega_p;
+
+    // Ensure minimum angular velocity for responsiveness
+    double omega_min = min_vel_theta_;
+    if (std::fabs(omega_target) < omega_min && abs_heading_error > final_heading_angle_tolerance_)
+    {
+      omega_target = heading_sign * omega_min;
+    }
+  }
+
+  // --- Apply acceleration limits ---
+  // Linear
+  double v_current = velocity.x;
+  double dv_max = acc_lim_x_ * dt;
+  double dv = std::clamp(v_target - v_current, -dv_max, dv_max);
+  cmd_vel.x = v_current + dv;
+
+  // Angular
+  double omega_current = velocity.theta;
+  double domega_max = max_vel_theta_;
+  double domega = std::clamp(omega_target - omega_current, -domega_max, domega_max);
+  cmd_vel.theta = omega_current + domega;
+
+  // --- Apply velocity limits ---
+  cmd_vel.x = std::clamp(cmd_vel.x, -min_speed_xy_, min_speed_xy_);
+  cmd_vel.theta = std::clamp(cmd_vel.theta, -max_vel_theta_, max_vel_theta_);
+
+  // --- Safety: ensure we can stop ---
+  double max_v_to_stop = std::sqrt(2.0 * std::fabs(decel_lim_x_) * xy_error);
+  if (std::fabs(cmd_vel.x) > max_v_to_stop)
+  {
+    cmd_vel.x = std::copysign(max_v_to_stop, cmd_vel.x);
+  }
+
+  cmd_vel.y = 0.0;
+
+  ROS_DEBUG_THROTTLE(0.2, "[FinalHeading] xy=%.3f, heading=%.3f, v=%.3f, omega=%.3f",
+                     xy_error, heading_error, cmd_vel.x, cmd_vel.theta);
+}
+
 double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity)
 {
   // If using velocity-scaled look ahead distances, find and clamp the dist
@@ -832,67 +896,109 @@ double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const rob
   return lookahead_dist;
 }
 
+// std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
+// mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan)
+// {
+//   if (global_plan.poses.empty())
+//   {
+//     throw robot_nav_core2::PlannerTFException("The global plan passed to the local planner is empty.");
+//   }
+
+//   if ((unsigned)global_plan.poses.size() < 2)
+//   {
+//     auto goal_pose_it = std::prev(global_plan.poses.end());
+//     return goal_pose_it;
+//   }
+
+//   unsigned int goal_index = (unsigned)global_plan.poses.size() - 1;
+  
+//   double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x;
+//   double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y;
+//   double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x;
+//   double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y;
+
+//   // make sure that atan2 is defined
+//   double start_angle = atan2(start_direction_y, start_direction_x);
+//   double end_angle = atan2(end_direction_y, end_direction_x);
+//   double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2);
+
+//   for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++)
+//   {
+//     if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9)
+//     {
+//       const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x;
+//       const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y;
+
+//       if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
+//       {
+//         double current_angle = atan2(current_direction_y, current_direction_x);
+//         goal_index = i;
+//         if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_))
+//           continue;
+
+//         if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold)
+//         {
+//           goal_index = i;
+//           break;
+//         }
+//       }
+//     }
+//   }
+
+//   // Find the first pose which is at a distance greater than the lookahead distance
+//   auto goal_pose_it = std::find_if(
+//   global_plan.poses.begin(), global_plan.poses.begin() + goal_index,
+//   [&](const auto &ps)
+//   {
+//     return std::hypot(ps.pose.x, ps.pose.y) >= lookahead_dist;
+//   });
+
+//   // If the number of poses is not far enough, take the last pose
+//   if (goal_pose_it == global_plan.poses.end())
+//   {
+//     goal_pose_it = std::prev(global_plan.poses.end());
+//   }
+//   return goal_pose_it;
+// }
+
 std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
 mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan)
 {
   if (global_plan.poses.empty())
-  {
     throw robot_nav_core2::PlannerTFException("The global plan passed to the local planner is empty.");
-  }
 
-  if ((unsigned)global_plan.poses.size() < 2)
-  {
-    auto goal_pose_it = std::prev(global_plan.poses.end());
-    return goal_pose_it;
-  }
+  if (global_plan.poses.size() < 2)
+    return std::prev(global_plan.poses.end());
 
   unsigned int goal_index = (unsigned)global_plan.poses.size() - 1;
-  // double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x;
-  // double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y;
-  // double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x;
-  // double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y;
 
-  // // make sure that atan2 is defined
-  // double start_angle = atan2(start_direction_y, start_direction_x);
-  // double end_angle = atan2(end_direction_y, end_direction_x);
-  // double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2);
+  // Góc đầu và ngưỡng gãy (fixed)
+  const auto &p0 = global_plan.poses[0].pose;
+  const auto &p1 = global_plan.poses[1].pose;
+  double start_angle = atan2(p1.y - p0.y, p1.x - p0.x);
+  double turn_threshold = M_PI_2 * 0.6; // ~54deg, chỉnh theo nhu cầu
 
-  // for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++)
-  // {
-  //   if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9)
-  //   {
-  //     const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x;
-  //     const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y;
-
-  //     if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
-  //     {
-  //       double current_angle = atan2(current_direction_y, current_direction_x);
-  //       goal_index = i;
-  //       if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_))
-  //         continue;
-
-  //       if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold)
-  //       {
-  //         goal_index = i;
-  //         break;
-  //       }
-  //     }
-  //   }
-  // }
-
-  // Find the first pose which is at a distance greater than the lookahead distance
-  auto goal_pose_it = std::find_if(
-  global_plan.poses.begin(), global_plan.poses.begin() + goal_index,
-  [&](const auto &ps)
+  // Dừng nếu gãy góc
+  for (unsigned int i = 1; i < global_plan.poses.size(); ++i)
   {
-    return std::hypot(ps.pose.x, ps.pose.y) >= lookahead_dist;
-  });
-
-  // If the number of poses is not far enough, take the last pose
-  if (goal_pose_it == global_plan.poses.end())
-  {
-    goal_pose_it = std::prev(global_plan.poses.end());
+    const auto &a = global_plan.poses[i - 1].pose;
+    const auto &b = global_plan.poses[i].pose;
+    double current_angle = atan2(b.y - a.y, b.x - a.x);
+    double delta = angles::normalize_angle(current_angle - start_angle);
+    goal_index = i;
+    if (fabs(delta) >= turn_threshold)
+      break;
   }
+  // Chọn điểm lookahead theo khoảng cách
+  auto goal_pose_it = std::find_if(
+      global_plan.poses.begin(), global_plan.poses.begin() + goal_index,
+      [&](const auto &ps) {
+        return std::hypot(ps.pose.x, ps.pose.y) >= lookahead_dist;
+      });
+
+  if (goal_pose_it == global_plan.poses.end())
+    goal_pose_it = std::prev(global_plan.poses.end());
+
   return goal_pose_it;
 }
 
@@ -942,19 +1048,112 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
   return true;
 }
 
-robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose)
+double mkt_algorithm::diff::PredictiveTrajectory::calculateMaxKappa(const robot_nav_2d_msgs::Path2D &path, const double preview_distance)
+{
+  double traveled = 0.0;
+  double max_kappa = 0.0;
+  for (size_t i = 1; i + 1 < path.poses.size(); ++i)
+  {
+    const auto &p0 = path.poses[i - 1].pose;
+    const auto &p1 = path.poses[i].pose;
+    const auto &p2 = path.poses[i + 1].pose;
+
+    const double a = std::hypot(p1.x - p0.x, p1.y - p0.y);
+    const double b = std::hypot(p2.x - p1.x, p2.y - p1.y);
+    const double c = std::hypot(p2.x - p0.x, p2.y - p0.y);
+    traveled += a;
+
+    if (a > 1e-6 && b > 1e-6 && c > 1e-6)
+    {
+      const double cross = (p1.x - p0.x) * (p2.y - p0.y) - (p1.y - p0.y) * (p2.x - p0.x);
+      const double area2 = std::fabs(cross); // 2 * area
+      const double kappa = 2.0 * area2 / (a * b * c);
+      max_kappa = std::max(max_kappa, std::fabs(kappa));
+    }
+
+    if (traveled >= preview_distance)
+      break;
+  }
+  return max_kappa;
+}
+
+double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajectory(
+  const robot_nav_2d_msgs::Twist2D &velocity,
+  const robot_nav_2d_msgs::Path2D &trajectory,
+  double v_target,
+  const double &sign_x)
+{
+  if (journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
+    return min_speed_xy_ * sign_x;
+
+  double preview_distance = std::clamp(std::fabs(velocity.x) * lookahead_time_, min_lookahead_dist_, max_lookahead_dist_);
+
+  double max_kappa = calculateMaxKappa(trajectory, preview_distance);
+  double v_limit = std::fabs(v_target);
+  if (max_kappa > 1e-6 && max_lateral_accel_ > 1e-6)
+  {
+    const double v_curve = std::sqrt(max_lateral_accel_ / max_kappa);
+    v_limit = std::min(v_limit, v_curve);
+  }
+  if (fabs(decel_lim_x_) > 1e-6)
+  {
+    const double remaining = journey(trajectory.poses, 0, trajectory.poses.size() - 1);
+    const double v_stop = std::sqrt(2.0 * fabs(decel_lim_x_) * std::max(0.0, remaining));
+    v_limit = std::min(v_limit, v_stop);
+  }
+
+  return std::copysign(v_limit, sign_x);
+}
+
+robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTrajectory(
+  const robot_nav_2d_msgs::Path2D &path, 
+  const robot_nav_2d_msgs::Twist2D &drive_target,
+  const robot_nav_2d_msgs::Twist2D &velocity,
+  const double &sign_x,
+  robot_nav_2d_msgs::Twist2D &drive_cmd)
+{
+  if (path.poses.empty())
+  {
+    drive_cmd.x = 0.0;
+    drive_cmd.theta = 0.0;
+    return robot_nav_2d_msgs::Path2D();
+  }
+  double max_kappa = calculateMaxKappa(path);
+  const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05));
+  drive_cmd.x = this->adjustSpeedWithHermiteTrajectory(velocity, path, drive_target.x, sign_x);
+  drive_cmd.theta = max_vel_theta_;
+  if (max_kappa <= straight_threshold)
+  { 
+    if(journey(path.poses, 0, path.poses.size() - 1) <= min_journey_squared_)
+    {
+      drive_cmd.theta = 0.05;
+      // return path;
+    }
+    return generateHermiteTrajectory(path.poses.back(), sign_x);
+    
+  }
+  else
+  {
+    if(fabs(drive_cmd.x) < min_speed_xy_)
+      drive_cmd.x = std::copysign(min_speed_xy_, sign_x);
+    return generateHermiteQuadraticTrajectory(path.poses.back(), sign_x);
+  }
+}
+
+robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(
+  const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x)
 {
   robot_nav_2d_msgs::Path2D hermite_trajectory;
   hermite_trajectory.poses.clear();
   hermite_trajectory.header.stamp = pose.header.stamp;
   hermite_trajectory.header.frame_id = pose.header.frame_id;
-  // Characteristic length (can be tuned)
+
   const double x = pose.pose.x;
   const double y = pose.pose.y;
   const double theta = pose.pose.theta;
-  double L = std::sqrt(x * x + y * y);
+  const double L = std::hypot(x, y);
+
   if (L < 1e-6) {
-    // Degenerate: return single pose
     robot_nav_2d_msgs::Pose2DStamped pose_stamped;
     pose_stamped.pose.x = 0.0;
     pose_stamped.pose.y = 0.0;
@@ -964,12 +1163,13 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer
     hermite_trajectory.poses.push_back(pose_stamped);
     return hermite_trajectory;
   }
+
   int samples = L/costmap_robot_->getCostmap()->getResolution();
   if (samples < 1) {
     samples = 1;
   }
   hermite_trajectory.poses.reserve(samples + 1);
-
+  const double Lnegative = sign_x < 0 ? -L : L;
   for (int i = 0; i <= samples; ++i) {
     double t = static_cast<double>(i) / samples;
     double t2 = t * t;
@@ -981,8 +1181,8 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer
     double h11 =      t3 -     t2;
 
     // Hermite interpolation
-    double px = h10 * L + h01 * x + h11 * (L * std::cos(theta));
-    double py = h01 * y + h11 * (L * std::sin(theta));
+    double px = h10 * Lnegative + h01 * x + h11 * Lnegative * std::cos(theta);
+    double py = h01 * y + h11 * Lnegative * std::sin(theta);
 
     // First derivatives for heading
     double dh00 = 6 * t2 - 6 * t;
@@ -990,15 +1190,15 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer
     double dh01 = -6 * t2 + 6 * t;
     double dh11 = 3 * t2 - 2 * t;
 
-    double dx = dh10 * L + dh01 * x + dh11 * (L * std::cos(theta));
-    double dy = dh01 * y + dh11 * (L * std::sin(theta));
+    double dx = dh10 * Lnegative + dh01 * x + dh11 * Lnegative * std::cos(theta);
+    double dy = dh01 * y + dh11 * Lnegative * std::sin(theta);
 
     double heading = std::atan2(dy, dx);
 
     robot_nav_2d_msgs::Pose2DStamped pose;
     pose.pose.x = px;
     pose.pose.y = py;
-    pose.pose.theta = heading;
+    pose.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading;
     pose.header.stamp = hermite_trajectory.header.stamp;
     pose.header.frame_id = hermite_trajectory.header.frame_id;
     hermite_trajectory.poses.push_back(pose);
@@ -1006,6 +1206,70 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer
   return hermite_trajectory;
 }
 
+robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteQuadraticTrajectory(
+  const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x)
+{
+  robot_nav_2d_msgs::Path2D trajectory;
+  trajectory.poses.clear();
+  trajectory.header.stamp = pose.header.stamp;
+  trajectory.header.frame_id = pose.header.frame_id;
+
+  const double x = pose.pose.x;
+  const double y = pose.pose.y;
+  const double theta = sign_x < 0 ? angles::normalize_angle(pose.pose.theta + M_PI) : pose.pose.theta;
+  const double L = std::hypot(x, y);
+  if (L < 1e-6)
+  {
+    robot_nav_2d_msgs::Pose2DStamped pose_stamped;
+    pose_stamped.pose.x = 0.0;
+    pose_stamped.pose.y = 0.0;
+    pose_stamped.pose.theta = 0.0;
+    pose_stamped.header.stamp = trajectory.header.stamp;
+    pose_stamped.header.frame_id = trajectory.header.frame_id;
+    trajectory.poses.push_back(pose_stamped);
+    return trajectory;
+  }
+
+  int samples = L/costmap_robot_->getCostmap()->getResolution();
+  if (samples < 1) {
+    samples = 1;
+  }
+  trajectory.poses.reserve(samples + 1);
+
+  // Quadratic Hermite with end tangent aligned to carrot_pose heading:
+  // p(0) = 0, p(1) = goal, p'(1) = v1
+  const double v1x = L * std::cos(theta);
+  const double v1y = L * std::sin(theta);
+  const double ax = v1x - x;
+  const double ay = v1y - y;
+  const double bx = 2.0 * x - v1x;
+  const double by = 2.0 * y - v1y;
+
+  trajectory.poses.reserve(samples + 1);
+  for (int i = 0; i <= samples; ++i)
+  {
+    double t = static_cast<double>(i) / samples;
+    double t2 = t * t;
+
+    double px = ax * t2 + bx * t;
+    double py = ay * t2 + by * t;
+
+    double dx = 2.0 * ax * t + bx;
+    double dy = 2.0 * ay * t + by;
+    double heading = std::atan2(dy, dx);
+
+    robot_nav_2d_msgs::Pose2DStamped pose_out;
+    pose_out.pose.x = px;
+    pose_out.pose.y = py;
+    pose_out.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading;
+    pose_out.header.stamp = trajectory.header.stamp;
+    pose_out.header.frame_id = trajectory.header.frame_id;
+    trajectory.poses.push_back(pose_out);
+  }
+
+  return trajectory;
+}
+
 bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
     TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose,
     const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
@@ -1182,7 +1446,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const robot_na
 }
 
 std::vector<robot_geometry_msgs::Point>
-mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution)
+mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(const robot_geometry_msgs::Pose2D &pose, const std::vector<robot_geometry_msgs::Point> &footprint, const double &resolution)
 {
 
   // Dịch sang tọa độ tuyệt đối
@@ -1235,34 +1499,6 @@ double mkt_algorithm::diff::PredictiveTrajectory::costAtPose(const double &x, co
   return static_cast<double>(cost);
 }
 
-
-double mkt_algorithm::diff::PredictiveTrajectory::computeDecelerationFactor(double remaining_distance, double decel_distance)
-{
-  if (remaining_distance >= decel_distance)
-  {
-    return 1.0; // Full speed
-  }
-
-  // Smooth transition using cosine function
-  double ratio = fabs(remaining_distance / decel_distance);
-  return 0.5 * (1.0 + cos(M_PI * (1.0 - ratio))); // Smooth from 1 to 0
-}
-
-double mkt_algorithm::diff::PredictiveTrajectory::getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
-                                                                       double journey_plan)
-{
-  double carrot_distance = sqrt(carrot_pose.pose.x * carrot_pose.pose.x +
-                                carrot_pose.pose.y * carrot_pose.pose.y);
-
-  // Avoid division by zero and handle backward motion
-  if (carrot_distance < 1e-3)
-    return journey_plan;
-
-  // Project remaining path onto carrot direction
-  double alignment = fabs(carrot_pose.pose.x / carrot_distance); // cos(angle)
-  return journey_plan * std::max(0.0, alignment);                // Only positive projection
-}
-
 score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create()
 {
   return std::make_shared<mkt_algorithm::diff::PredictiveTrajectory>();
diff --git a/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp b/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp
index 5af06df..f11251d 100644
--- a/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp
+++ b/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp
@@ -194,7 +194,6 @@ namespace mkt_plugins
   void KinematicParameters::setMaxTheta(double value)
   {
     max_vel_theta_  = fabs(value) <= fabs(original_max_vel_theta_) + EPSILON? value : original_max_vel_theta_;
-    // ROS_WARN_THROTTLE(10, "vtheta %f %f %f", value, original_max_vel_theta_, max_vel_theta_);
   }
 
   void KinematicParameters::setAccTheta(double value)
diff --git a/src/Algorithms/Packages/global_planners/custom_planner b/src/Algorithms/Packages/global_planners/custom_planner
index 49afcce..e0f6738 160000
--- a/src/Algorithms/Packages/global_planners/custom_planner
+++ b/src/Algorithms/Packages/global_planners/custom_planner
@@ -1 +1 @@
-Subproject commit 49afcce5b2247793282fe5e94d0e27f062562325
+Subproject commit e0f6738c3161b7a4a9418d960d7e419a43f7d521
diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp
index f4725da..f261959 100644
--- a/src/Navigations/Packages/move_base/src/move_base.cpp
+++ b/src/Navigations/Packages/move_base/src/move_base.cpp
@@ -221,7 +221,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     try
     {
       robot::PluginLoaderHelper loader;
-      std::string path_file_so = loader.findLibraryPath("CustomPlanner");
+      std::string path_file_so = loader.findLibraryPath(global_planner);
       planner_loader_ = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
           path_file_so, global_planner, boost::dll::load_mode::append_decorations);
 
@@ -257,7 +257,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     try
     {
       robot::PluginLoaderHelper loader;
-      std::string path_file_so = loader.findLibraryPath("LocalPlannerAdapter");
+      std::string path_file_so = loader.findLibraryPath(local_planner);
       controller_loader_ =
           boost::dll::import_alias<robot_nav_core::BaseLocalPlanner::Ptr()>(
               path_file_so, local_planner, boost::dll::load_mode::append_decorations);