From 9c29521b8b9842b93ae11100faf52c699b4966bd Mon Sep 17 00:00:00 2001
From: Sakshay Mahna <sakshum19@gmail.com>
Date: Fri, 5 Sep 2025 14:20:35 +0530
Subject: [PATCH 1/6] Update smooth control law

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>
---
 .../smooth_control_law.hpp                         |  5 ++++-
 .../src/smooth_control_law.cpp                     | 14 +++++++++-----
 2 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp
index eb553d3927c..a647ddea7d7 100644
--- a/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp
+++ b/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp
@@ -43,10 +43,11 @@ class SmoothControlLaw
    * @param v_linear_min Minimum linear velocity.
    * @param v_linear_max Maximum linear velocity.
    * @param v_angular_max Maximum angular velocity.
+   * @param deceleration_max Maximum deceleration.
    */
   SmoothControlLaw(
     double k_phi, double k_delta, double beta, double lambda, double slowdown_radius,
-    double v_linear_min, double v_linear_max, double v_angular_max);
+    double v_linear_min, double v_linear_max, double v_angular_max, double deceleration_max);
 
   /**
    * @brief Destructor for nav2_graceful_controller::SmoothControlLaw
@@ -86,12 +87,14 @@ class SmoothControlLaw
    *
    * @param target Pose of the target in the robot frame.
    * @param current Current pose of the robot in the robot frame.
+   * @param target_distance Path distance from current to target frame.
    * @param backward If true, the robot is moving backwards. Defaults to false.
    * @return Velocity command.
    */
   geometry_msgs::msg::Twist calculateRegularVelocity(
     const geometry_msgs::msg::Pose & target,
     const geometry_msgs::msg::Pose & current,
+    const double & target_distance,
     const bool & backward = false);
 
   /**
diff --git a/nav2_graceful_controller/src/smooth_control_law.cpp b/nav2_graceful_controller/src/smooth_control_law.cpp
index 7a04fce71ef..e897c5dc6f0 100644
--- a/nav2_graceful_controller/src/smooth_control_law.cpp
+++ b/nav2_graceful_controller/src/smooth_control_law.cpp
@@ -22,9 +22,10 @@ namespace nav2_graceful_controller
 
 SmoothControlLaw::SmoothControlLaw(
   double k_phi, double k_delta, double beta, double lambda, double slowdown_radius,
-  double v_linear_min, double v_linear_max, double v_angular_max)
+  double v_linear_min, double v_linear_max, double v_angular_max, double deceleration_max)
 : k_phi_(k_phi), k_delta_(k_delta), beta_(beta), lambda_(lambda), slowdown_radius_(slowdown_radius),
-  v_linear_min_(v_linear_min), v_linear_max_(v_linear_max), v_angular_max_(v_angular_max)
+  v_linear_min_(v_linear_min), v_linear_max_(v_linear_max), v_angular_max_(v_angular_max), 
+  deceleration_max_(deceleration_max)
 {
 }
 
@@ -52,7 +53,7 @@ void SmoothControlLaw::setSpeedLimit(
 
 geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
   const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
-  const bool & backward)
+  const double & target_distance, const bool & backward)
 {
   // Convert the target to polar coordinates
   auto ego_coords = EgocentricPolarCoordinates(target, current, backward);
@@ -66,7 +67,10 @@ geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
   double v = v_linear_max_ / (1.0 + beta_ * std::pow(fabs(curvature), lambda_));
 
   // Slowdown when the robot is near the target to remove singularity
-  v = std::min(v_linear_max_ * (ego_coords.r / slowdown_radius_), v);
+  v = std::min(v_linear_max_ * (target_distance / slowdown_radius_), v);
+
+  // Constraint robot velocity within deceleration limits
+  v = std::min(sqrt(2 * target_distance * deceleration_max_), v);
 
   // Set some small v_min when far away from origin to promote faster
   // turning motion when the curvature is very high
@@ -92,7 +96,7 @@ geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
 geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
   const geometry_msgs::msg::Pose & target, const bool & backward)
 {
-  return calculateRegularVelocity(target, geometry_msgs::msg::Pose(), backward);
+  return calculateRegularVelocity(target, geometry_msgs::msg::Pose(), 0, backward);
 }
 
 geometry_msgs::msg::Pose SmoothControlLaw::calculateNextPose(

From fce731df9cceb34eb904aa7c9c8d8a8dbdca83ae Mon Sep 17 00:00:00 2001
From: Sakshay Mahna <sakshum19@gmail.com>
Date: Fri, 5 Sep 2025 14:36:09 +0530
Subject: [PATCH 2/6] Update Graceful controller

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>
---
 .../nav2_graceful_controller/graceful_controller.hpp       | 2 ++
 .../nav2_graceful_controller/smooth_control_law.hpp        | 2 ++
 nav2_graceful_controller/src/graceful_controller.cpp       | 7 ++++---
 nav2_graceful_controller/src/smooth_control_law.cpp        | 7 +++++--
 4 files changed, 13 insertions(+), 5 deletions(-)

diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp
index 28bec7eddd8..ef65c380d17 100644
--- a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp
+++ b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp
@@ -131,6 +131,7 @@ class GracefulController : public nav2_core::Controller
    *
    * @param motion_target Motion target point (in costmap local frame?)
    * @param costmap_transform Transform between global and local costmap
+   * @param target_distance Path distance to target point
    * @param trajectory Simulated trajectory
    * @param cmd_vel Initial command velocity during simulation
    * @param backward Flag to indicate if the robot is moving backward
@@ -139,6 +140,7 @@ class GracefulController : public nav2_core::Controller
   bool simulateTrajectory(
     const geometry_msgs::msg::PoseStamped & motion_target,
     const geometry_msgs::msg::TransformStamped & costmap_transform,
+    const double & target_distance,
     nav_msgs::msg::Path & trajectory,
     geometry_msgs::msg::TwistStamped & cmd_vel,
     bool backward);
diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp
index a647ddea7d7..5d18301836b 100644
--- a/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp
+++ b/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp
@@ -114,6 +114,7 @@ class SmoothControlLaw
    * @param dt Time step.
    * @param target Pose of the target in the robot frame.
    * @param current Current pose of the robot in the robot frame.
+   * @param target_distance Path distance from current to target frame.
    * @param backward If true, the robot is moving backwards. Defaults to false.
    * @return geometry_msgs::msg::Pose
    */
@@ -121,6 +122,7 @@ class SmoothControlLaw
     const double dt,
     const geometry_msgs::msg::Pose & target,
     const geometry_msgs::msg::Pose & current,
+    const double & target_distance,
     const bool & backward = false);
 
 protected:
diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp
index b8cce0b0aed..479d692e7ce 100644
--- a/nav2_graceful_controller/src/graceful_controller.cpp
+++ b/nav2_graceful_controller/src/graceful_controller.cpp
@@ -310,7 +310,7 @@ bool GracefulController::validateTargetPose(
   }
 
   // Actually simulate the path
-  if (simulateTrajectory(target_pose, costmap_transform, trajectory, cmd_vel, reversing)) {
+  if (simulateTrajectory(target_pose, costmap_transform, dist_to_target, trajectory, cmd_vel, reversing)) {
     // Successfully simulated to target_pose
     return true;
   }
@@ -322,6 +322,7 @@ bool GracefulController::validateTargetPose(
 bool GracefulController::simulateTrajectory(
   const geometry_msgs::msg::PoseStamped & motion_target,
   const geometry_msgs::msg::TransformStamped & costmap_transform,
+  const double & target_distance,
   nav_msgs::msg::Path & trajectory,
   geometry_msgs::msg::TwistStamped & cmd_vel,
   bool backward)
@@ -372,12 +373,12 @@ bool GracefulController::simulateTrajectory(
       // If this is first iteration, this is our current target velocity
       if (trajectory.poses.empty()) {
         cmd_vel.twist = control_law_->calculateRegularVelocity(
-          motion_target.pose, next_pose.pose, backward);
+          motion_target.pose, next_pose.pose, target_distance, backward);
       }
 
       // Apply velocities to calculate next pose
       next_pose.pose = control_law_->calculateNextPose(
-        dt, motion_target.pose, next_pose.pose, backward);
+        dt, motion_target.pose, next_pose.pose, target_distance, backward);
     }
 
     // Add the pose to the trajectory for visualization
diff --git a/nav2_graceful_controller/src/smooth_control_law.cpp b/nav2_graceful_controller/src/smooth_control_law.cpp
index e897c5dc6f0..03b4e94ce12 100644
--- a/nav2_graceful_controller/src/smooth_control_law.cpp
+++ b/nav2_graceful_controller/src/smooth_control_law.cpp
@@ -96,16 +96,19 @@ geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
 geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
   const geometry_msgs::msg::Pose & target, const bool & backward)
 {
-  return calculateRegularVelocity(target, geometry_msgs::msg::Pose(), 0, backward);
+  double current_pose = geometry_msgs::msg::Pose();
+  double target_distance = euclidean_distance(target, current_pose);
+  return calculateRegularVelocity(target, current_pose, target_distance, backward);
 }
 
 geometry_msgs::msg::Pose SmoothControlLaw::calculateNextPose(
   const double dt,
   const geometry_msgs::msg::Pose & target,
   const geometry_msgs::msg::Pose & current,
+  const double & target_distance,
   const bool & backward)
 {
-  geometry_msgs::msg::Twist vel = calculateRegularVelocity(target, current, backward);
+  geometry_msgs::msg::Twist vel = calculateRegularVelocity(target, current, target_distance, backward);
   geometry_msgs::msg::Pose next;
   double yaw = tf2::getYaw(current.orientation);
   next.position.x = current.position.x + vel.linear.x * dt * cos(yaw);

From a243818fa65aa4e9611302b66eafdb7fe1766eff Mon Sep 17 00:00:00 2001
From: Sakshay Mahna <sakshum19@gmail.com>
Date: Fri, 5 Sep 2025 15:23:31 +0530
Subject: [PATCH 3/6] Fix build issues

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>
---
 .../include/opennav_docking/controller.hpp    |  3 +-
 .../opennav_docking/src/controller.cpp        |  7 ++--
 .../parameter_handler.hpp                     |  1 +
 .../smooth_control_law.hpp                    | 19 ++++++++--
 .../src/graceful_controller.cpp               |  8 +++--
 .../src/parameter_handler.cpp                 |  5 +++
 .../src/smooth_control_law.cpp                | 23 +++++++-----
 .../test/test_graceful_controller.cpp         | 36 +++++++++++++------
 8 files changed, 76 insertions(+), 26 deletions(-)

diff --git a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp
index efb8f6ccf5b..bce31140f9d 100644
--- a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp
+++ b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp
@@ -121,7 +121,8 @@ class Controller
   // Smooth control law
   std::unique_ptr<nav2_graceful_controller::SmoothControlLaw> control_law_;
   double k_phi_, k_delta_, beta_, lambda_;
-  double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_;
+  double slowdown_radius_, deceleration_max_;
+  double v_linear_min_, v_linear_max_, v_angular_max_;
   double rotate_to_heading_angular_vel_, rotate_to_heading_max_angular_accel_;
 
   // The trajectory of the robot while dock / undock for visualization / debug purposes
diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp
index 7838158f8af..ee9f6d5db07 100644
--- a/nav2_docking/opennav_docking/src/controller.cpp
+++ b/nav2_docking/opennav_docking/src/controller.cpp
@@ -48,6 +48,8 @@ Controller::Controller(
     node, "controller.v_angular_max", rclcpp::ParameterValue(0.75));
   nav2::declare_parameter_if_not_declared(
     node, "controller.slowdown_radius", rclcpp::ParameterValue(0.25));
+  nav2::declare_parameter_if_not_declared(
+    node, "controller.deceleration_max", rclcpp::ParameterValue(0.1));
   nav2::declare_parameter_if_not_declared(
       node, "controller.rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.0));
   nav2::declare_parameter_if_not_declared(
@@ -77,9 +79,10 @@ Controller::Controller(
   node->get_parameter("controller.v_linear_max", v_linear_max_);
   node->get_parameter("controller.v_angular_max", v_angular_max_);
   node->get_parameter("controller.slowdown_radius", slowdown_radius_);
+  node->get_parameter("controller.deceleration_max", deceleration_max_);
   control_law_ = std::make_unique<nav2_graceful_controller::SmoothControlLaw>(
-    k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, v_linear_min_, v_linear_max_,
-    v_angular_max_);
+    k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, deceleration_max_,
+    v_linear_min_, v_linear_max_, v_angular_max_);
 
   // Add callback for dynamic parameters
   dyn_params_handler_ = node->add_on_set_parameters_callback(
diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp
index 8d22841f817..e1bb936f6e0 100644
--- a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp
+++ b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp
@@ -46,6 +46,7 @@ struct Parameters
   double v_angular_max_initial;
   double v_angular_min_in_place;
   double slowdown_radius;
+  double deceleration_max;
   bool initial_rotation;
   double initial_rotation_tolerance;
   bool prefer_final_rotation;
diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp
index 5d18301836b..4ad66c45356 100644
--- a/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp
+++ b/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp
@@ -40,14 +40,15 @@ class SmoothControlLaw
    * @param beta Constant factor applied to the path curvature: dropping velocity.
    * @param lambda Constant factor applied to the path curvature for sharpness.
    * @param slowdown_radius Radial threshold applied to the slowdown rule.
+   * @param deceleration_max Maximum deceleration.
    * @param v_linear_min Minimum linear velocity.
    * @param v_linear_max Maximum linear velocity.
    * @param v_angular_max Maximum angular velocity.
-   * @param deceleration_max Maximum deceleration.
    */
   SmoothControlLaw(
-    double k_phi, double k_delta, double beta, double lambda, double slowdown_radius,
-    double v_linear_min, double v_linear_max, double v_angular_max, double deceleration_max);
+    double k_phi, double k_delta, double beta, double lambda,
+    double slowdown_radius, double deceleration_max,
+    double v_linear_min, double v_linear_max, double v_angular_max);
 
   /**
    * @brief Destructor for nav2_graceful_controller::SmoothControlLaw
@@ -72,6 +73,13 @@ class SmoothControlLaw
    */
   void setSlowdownRadius(const double slowdown_radius);
 
+  /**
+   * @brief Set the max deceleration
+   *
+   * @param deceleration_max Maximum deceleration possible.
+   */
+  void setMaxDeceleration(const double deceleration_max);
+
   /**
    * @brief Update the velocity limits.
    *
@@ -175,6 +183,11 @@ class SmoothControlLaw
    */
   double slowdown_radius_;
 
+  /**
+   * @brief Maximum deceleration.
+   */
+  double deceleration_max_;
+
   /**
    * @brief Minimum linear velocity.
    */
diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp
index 479d692e7ce..ee0550a1eea 100644
--- a/nav2_graceful_controller/src/graceful_controller.cpp
+++ b/nav2_graceful_controller/src/graceful_controller.cpp
@@ -53,7 +53,8 @@ void GracefulController::configure(
 
   // Handles the control law to generate the velocity commands
   control_law_ = std::make_unique<SmoothControlLaw>(
-    params_->k_phi, params_->k_delta, params_->beta, params_->lambda, params_->slowdown_radius,
+    params_->k_phi, params_->k_delta, params_->beta, params_->lambda,
+    params_->slowdown_radius, params_->deceleration_max,
     params_->v_linear_min, params_->v_linear_max, params_->v_angular_max);
 
   // Initialize footprint collision checker
@@ -134,6 +135,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
   control_law_->setCurvatureConstants(
     params_->k_phi, params_->k_delta, params_->beta, params_->lambda);
   control_law_->setSlowdownRadius(params_->slowdown_radius);
+  control_law_->setMaxDeceleration(params_->deceleration_max);
   control_law_->setSpeedLimit(params_->v_linear_min, params_->v_linear_max, params_->v_angular_max);
 
   // Transform path to robot base frame
@@ -310,7 +312,9 @@ bool GracefulController::validateTargetPose(
   }
 
   // Actually simulate the path
-  if (simulateTrajectory(target_pose, costmap_transform, dist_to_target, trajectory, cmd_vel, reversing)) {
+  if (simulateTrajectory(target_pose, costmap_transform, dist_to_target, trajectory, cmd_vel,
+      reversing))
+  {
     // Successfully simulated to target_pose
     return true;
   }
diff --git a/nav2_graceful_controller/src/parameter_handler.cpp b/nav2_graceful_controller/src/parameter_handler.cpp
index 95fdda9f82b..8babb79e80e 100644
--- a/nav2_graceful_controller/src/parameter_handler.cpp
+++ b/nav2_graceful_controller/src/parameter_handler.cpp
@@ -58,6 +58,8 @@ ParameterHandler::ParameterHandler(
     node, plugin_name_ + ".v_angular_min_in_place", rclcpp::ParameterValue(0.25));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".slowdown_radius", rclcpp::ParameterValue(1.5));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".deceleration_max", rclcpp::ParameterValue(3.0));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".initial_rotation", rclcpp::ParameterValue(true));
   declare_parameter_if_not_declared(
@@ -97,6 +99,7 @@ ParameterHandler::ParameterHandler(
   node->get_parameter(
     plugin_name_ + ".v_angular_min_in_place", params_.v_angular_min_in_place);
   node->get_parameter(plugin_name_ + ".slowdown_radius", params_.slowdown_radius);
+  node->get_parameter(plugin_name_ + ".deceleration_max", params_.deceleration_max);
   node->get_parameter(plugin_name_ + ".initial_rotation", params_.initial_rotation);
   node->get_parameter(
     plugin_name_ + ".initial_rotation_tolerance", params_.initial_rotation_tolerance);
@@ -168,6 +171,8 @@ ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> param
         params_.v_angular_min_in_place = parameter.as_double();
       } else if (param_name == plugin_name_ + ".slowdown_radius") {
         params_.slowdown_radius = parameter.as_double();
+      } else if (param_name == plugin_name_ + ".deceleration_max") {
+        params_.deceleration_max = parameter.as_double();
       } else if (param_name == plugin_name_ + ".initial_rotation_tolerance") {
         params_.initial_rotation_tolerance = parameter.as_double();
       } else if (param_name == plugin_name_ + ".rotation_scaling_factor") {
diff --git a/nav2_graceful_controller/src/smooth_control_law.cpp b/nav2_graceful_controller/src/smooth_control_law.cpp
index 03b4e94ce12..9092932d570 100644
--- a/nav2_graceful_controller/src/smooth_control_law.cpp
+++ b/nav2_graceful_controller/src/smooth_control_law.cpp
@@ -21,11 +21,12 @@ namespace nav2_graceful_controller
 {
 
 SmoothControlLaw::SmoothControlLaw(
-  double k_phi, double k_delta, double beta, double lambda, double slowdown_radius,
-  double v_linear_min, double v_linear_max, double v_angular_max, double deceleration_max)
-: k_phi_(k_phi), k_delta_(k_delta), beta_(beta), lambda_(lambda), slowdown_radius_(slowdown_radius),
-  v_linear_min_(v_linear_min), v_linear_max_(v_linear_max), v_angular_max_(v_angular_max), 
-  deceleration_max_(deceleration_max)
+  double k_phi, double k_delta, double beta, double lambda,
+  double slowdown_radius, double deceleration_max,
+  double v_linear_min, double v_linear_max, double v_angular_max)
+: k_phi_(k_phi), k_delta_(k_delta), beta_(beta), lambda_(lambda),
+  slowdown_radius_(slowdown_radius), deceleration_max_(deceleration_max),
+  v_linear_min_(v_linear_min), v_linear_max_(v_linear_max), v_angular_max_(v_angular_max)
 {
 }
 
@@ -43,6 +44,11 @@ void SmoothControlLaw::setSlowdownRadius(double slowdown_radius)
   slowdown_radius_ = slowdown_radius;
 }
 
+void SmoothControlLaw::setMaxDeceleration(double deceleration_max)
+{
+  deceleration_max_ = deceleration_max;
+}
+
 void SmoothControlLaw::setSpeedLimit(
   const double v_linear_min, const double v_linear_max, const double v_angular_max)
 {
@@ -96,8 +102,8 @@ geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
 geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
   const geometry_msgs::msg::Pose & target, const bool & backward)
 {
-  double current_pose = geometry_msgs::msg::Pose();
-  double target_distance = euclidean_distance(target, current_pose);
+  geometry_msgs::msg::Pose current_pose = geometry_msgs::msg::Pose();
+  double target_distance = nav2_util::geometry_utils::euclidean_distance(target, current_pose);
   return calculateRegularVelocity(target, current_pose, target_distance, backward);
 }
 
@@ -108,7 +114,8 @@ geometry_msgs::msg::Pose SmoothControlLaw::calculateNextPose(
   const double & target_distance,
   const bool & backward)
 {
-  geometry_msgs::msg::Twist vel = calculateRegularVelocity(target, current, target_distance, backward);
+  geometry_msgs::msg::Twist vel = calculateRegularVelocity(target, current, target_distance,
+      backward);
   geometry_msgs::msg::Pose next;
   double yaw = tf2::getYaw(current.orientation);
   next.position.x = current.position.x + vel.linear.x * dt * cos(yaw);
diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp
index 799daa8d8b3..fa8426b4317 100644
--- a/nav2_graceful_controller/test/test_graceful_controller.cpp
+++ b/nav2_graceful_controller/test/test_graceful_controller.cpp
@@ -27,15 +27,17 @@ class SCLFixture : public nav2_graceful_controller::SmoothControlLaw
 public:
   SCLFixture(
     double k_phi, double k_delta, double beta, double lambda,
-    double slowdown_radius, double v_linear_min, double v_linear_max, double v_angular_max)
+    double slowdown_radius, double deceleration_max,
+    double v_linear_min, double v_linear_max, double v_angular_max)
   : nav2_graceful_controller::SmoothControlLaw(k_phi, k_delta, beta, lambda,
-      slowdown_radius, v_linear_min, v_linear_max, v_angular_max) {}
+      slowdown_radius, deceleration_max, v_linear_min, v_linear_max, v_angular_max) {}
 
   double getCurvatureKPhi() {return k_phi_;}
   double getCurvatureKDelta() {return k_delta_;}
   double getCurvatureBeta() {return beta_;}
   double getCurvatureLambda() {return lambda_;}
   double getSlowdownRadius() {return slowdown_radius_;}
+  double getMaxDeceleration() {return deceleration_max_;}
   double getSpeedLinearMin() {return v_linear_min_;}
   double getSpeedLinearMax() {return v_linear_max_;}
   double getSpeedAngularMax() {return v_angular_max_;}
@@ -84,7 +86,7 @@ class GMControllerFixture : public nav2_graceful_controller::GracefulController
 
 TEST(SmoothControlLawTest, setCurvatureConstants) {
   // Initialize SmoothControlLaw
-  SCLFixture scl(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0, 0);
+  SCLFixture scl(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
 
   // Set curvature constants
   scl.setCurvatureConstants(1.0, 2.0, 3.0, 4.0);
@@ -100,9 +102,20 @@ TEST(SmoothControlLawTest, setCurvatureConstants) {
   EXPECT_EQ(scl.getSlowdownRadius(), 5.0);
 }
 
+TEST(SmoothControlLawTest, setMaxDeceleration) {
+  // Initialize SmoothControlLaw
+  SCLFixture scl(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+
+  // Set max deceleration
+  scl.setMaxDeceleration(3.0);
+
+  // Check results
+  EXPECT_EQ(scl.getMaxDeceleration(), 3.0);
+}
+
 TEST(SmoothControlLawTest, setSpeedLimits) {
   // Initialize SmoothControlLaw
-  SCLFixture scl(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+  SCLFixture scl(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
 
   // Set speed limits
   scl.setSpeedLimit(1.0, 2.0, 3.0);
@@ -115,7 +128,7 @@ TEST(SmoothControlLawTest, setSpeedLimits) {
 
 TEST(SmoothControlLawTest, calculateCurvature) {
   // Initialize SmoothControlLaw
-  SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 0.0, 1.0, 1.0);
+  SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 3.0, 0.0, 1.0, 1.0);
 
   // Initialize target
   geometry_msgs::msg::Pose target;
@@ -150,7 +163,7 @@ TEST(SmoothControlLawTest, calculateCurvature) {
 
 TEST(SmoothControlLawTest, calculateRegularVelocity) {
   // Initialize SmoothControlLaw
-  SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 0.0, 1.0, 1.0);
+  SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 3.0, 0.0, 1.0, 1.0);
 
   // Initialize target
   geometry_msgs::msg::Pose target;
@@ -163,7 +176,8 @@ TEST(SmoothControlLawTest, calculateRegularVelocity) {
   current.position.y = 0.0;
   current.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
   // Calculate velocity
-  auto cmd_vel = scl.calculateRegularVelocity(target, current);
+  double target_distance = 5.0;
+  auto cmd_vel = scl.calculateRegularVelocity(target, current, target_distance);
 
   // Check results: both linear and angular velocity must be positive
   EXPECT_NEAR(cmd_vel.linear.x, 0.1460446, 0.0001);
@@ -178,7 +192,8 @@ TEST(SmoothControlLawTest, calculateRegularVelocity) {
   current.position.y = 0.0;
   current.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
   // Calculate velocity
-  cmd_vel = scl.calculateRegularVelocity(target, current);
+  target_distance = 4.995;
+  cmd_vel = scl.calculateRegularVelocity(target, current, target_distance);
 
   // Check results: linear velocity must be positive and angular velocity must be negative
   EXPECT_NEAR(cmd_vel.linear.x, 0.96651200, 0.0001);
@@ -187,7 +202,7 @@ TEST(SmoothControlLawTest, calculateRegularVelocity) {
 
 TEST(SmoothControlLawTest, calculateNextPose) {
   // Initialize SmoothControlLaw
-  SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 0.0, 1.0, 1.0);
+  SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 3.0, 0.0, 1.0, 1.0);
 
   // Initialize target
   geometry_msgs::msg::Pose target;
@@ -201,7 +216,8 @@ TEST(SmoothControlLawTest, calculateNextPose) {
   current.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
   // Calculate next pose
   float dt = 0.1;
-  auto next_pose = scl.calculateNextPose(dt, target, current);
+  double target_distance = 5.0;
+  auto next_pose = scl.calculateNextPose(dt, target, current, target_distance);
 
   // Check results
   EXPECT_NEAR(next_pose.position.x, 0.1, 0.1);

From aac6f7325f151d76630c425fbceeef883dc09868 Mon Sep 17 00:00:00 2001
From: Sakshay Mahna <sakshum19@gmail.com>
Date: Sat, 20 Sep 2025 20:22:59 +0530
Subject: [PATCH 4/6] Update deceleration_max for docking

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>
---
 nav2_docking/opennav_docking/src/controller.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp
index ee9f6d5db07..1601f277d98 100644
--- a/nav2_docking/opennav_docking/src/controller.cpp
+++ b/nav2_docking/opennav_docking/src/controller.cpp
@@ -49,7 +49,7 @@ Controller::Controller(
   nav2::declare_parameter_if_not_declared(
     node, "controller.slowdown_radius", rclcpp::ParameterValue(0.25));
   nav2::declare_parameter_if_not_declared(
-    node, "controller.deceleration_max", rclcpp::ParameterValue(0.1));
+    node, "controller.deceleration_max", rclcpp::ParameterValue(3.0));
   nav2::declare_parameter_if_not_declared(
       node, "controller.rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.0));
   nav2::declare_parameter_if_not_declared(

From c74360aef1ced9300a78bed2e99cc34ba6da69f9 Mon Sep 17 00:00:00 2001
From: Sakshay Mahna <sakshum19@gmail.com>
Date: Fri, 26 Sep 2025 18:31:48 +0530
Subject: [PATCH 5/6] Fix control_law function calls in dock controller

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>
---
 nav2_docking/opennav_docking/src/controller.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp
index 1601f277d98..162a2792d81 100644
--- a/nav2_docking/opennav_docking/src/controller.cpp
+++ b/nav2_docking/opennav_docking/src/controller.cpp
@@ -184,8 +184,9 @@ bool Controller::isTrajectoryCollisionFree(
 
   do{
     // Apply velocities to calculate next pose
+    double target_distance = nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose);
     next_pose.pose = control_law_->calculateNextPose(
-      simulation_time_step_, target_pose, next_pose.pose, backward);
+      simulation_time_step_, target_pose, next_pose.pose, target_distance, backward);
 
     // Add the pose to the trajectory for visualization
     trajectory.poses.push_back(next_pose);
@@ -200,7 +201,7 @@ bool Controller::isTrajectoryCollisionFree(
     // and the initial segment for undocking
     // This avoids false positives when the robot is at the dock
     double dock_collision_distance = is_docking ?
-      nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose) :
+      target_distance :
       std::hypot(next_pose.pose.position.x, next_pose.pose.position.y);
 
     // If this distance is greater than the dock_collision_threshold, check for collisions
@@ -265,6 +266,8 @@ Controller::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
         v_angular_max_ = parameter.as_double();
       } else if (param_name == "controller.slowdown_radius") {
         slowdown_radius_ = parameter.as_double();
+      } else if (param_name == "controller.deceleration_max") {
+        deceleration_max_ = parameter.as_double();
       } else if (param_name == "controller.rotate_to_heading_angular_vel") {
         rotate_to_heading_angular_vel_ = parameter.as_double();
       } else if (param_name == "controller.rotate_to_heading_max_angular_accel") {
@@ -280,6 +283,7 @@ Controller::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
       // Update the smooth control law with the new params
       control_law_->setCurvatureConstants(k_phi_, k_delta_, beta_, lambda_);
       control_law_->setSlowdownRadius(slowdown_radius_);
+      control_law_->setMaxDeceleration(deceleration_max_);
       control_law_->setSpeedLimit(v_linear_min_, v_linear_max_, v_angular_max_);
     }
   }

From 0f734f140e6f858ab4efdb20433cb1e5ff224297 Mon Sep 17 00:00:00 2001
From: Sakshay Mahna <sakshum19@gmail.com>
Date: Mon, 29 Sep 2025 18:48:44 +0530
Subject: [PATCH 6/6] Isolate changes from nav2_docking

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>
---
 .../include/opennav_docking/controller.hpp    |  3 +-
 .../opennav_docking/src/controller.cpp        | 15 ++---
 .../smooth_control_law.hpp                    | 47 +++++++++++++
 .../src/smooth_control_law.cpp                | 67 ++++++++++++++++++-
 4 files changed, 116 insertions(+), 16 deletions(-)

diff --git a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp
index bce31140f9d..efb8f6ccf5b 100644
--- a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp
+++ b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp
@@ -121,8 +121,7 @@ class Controller
   // Smooth control law
   std::unique_ptr<nav2_graceful_controller::SmoothControlLaw> control_law_;
   double k_phi_, k_delta_, beta_, lambda_;
-  double slowdown_radius_, deceleration_max_;
-  double v_linear_min_, v_linear_max_, v_angular_max_;
+  double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_;
   double rotate_to_heading_angular_vel_, rotate_to_heading_max_angular_accel_;
 
   // The trajectory of the robot while dock / undock for visualization / debug purposes
diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp
index 162a2792d81..7838158f8af 100644
--- a/nav2_docking/opennav_docking/src/controller.cpp
+++ b/nav2_docking/opennav_docking/src/controller.cpp
@@ -48,8 +48,6 @@ Controller::Controller(
     node, "controller.v_angular_max", rclcpp::ParameterValue(0.75));
   nav2::declare_parameter_if_not_declared(
     node, "controller.slowdown_radius", rclcpp::ParameterValue(0.25));
-  nav2::declare_parameter_if_not_declared(
-    node, "controller.deceleration_max", rclcpp::ParameterValue(3.0));
   nav2::declare_parameter_if_not_declared(
       node, "controller.rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.0));
   nav2::declare_parameter_if_not_declared(
@@ -79,10 +77,9 @@ Controller::Controller(
   node->get_parameter("controller.v_linear_max", v_linear_max_);
   node->get_parameter("controller.v_angular_max", v_angular_max_);
   node->get_parameter("controller.slowdown_radius", slowdown_radius_);
-  node->get_parameter("controller.deceleration_max", deceleration_max_);
   control_law_ = std::make_unique<nav2_graceful_controller::SmoothControlLaw>(
-    k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, deceleration_max_,
-    v_linear_min_, v_linear_max_, v_angular_max_);
+    k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, v_linear_min_, v_linear_max_,
+    v_angular_max_);
 
   // Add callback for dynamic parameters
   dyn_params_handler_ = node->add_on_set_parameters_callback(
@@ -184,9 +181,8 @@ bool Controller::isTrajectoryCollisionFree(
 
   do{
     // Apply velocities to calculate next pose
-    double target_distance = nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose);
     next_pose.pose = control_law_->calculateNextPose(
-      simulation_time_step_, target_pose, next_pose.pose, target_distance, backward);
+      simulation_time_step_, target_pose, next_pose.pose, backward);
 
     // Add the pose to the trajectory for visualization
     trajectory.poses.push_back(next_pose);
@@ -201,7 +197,7 @@ bool Controller::isTrajectoryCollisionFree(
     // and the initial segment for undocking
     // This avoids false positives when the robot is at the dock
     double dock_collision_distance = is_docking ?
-      target_distance :
+      nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose) :
       std::hypot(next_pose.pose.position.x, next_pose.pose.position.y);
 
     // If this distance is greater than the dock_collision_threshold, check for collisions
@@ -266,8 +262,6 @@ Controller::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
         v_angular_max_ = parameter.as_double();
       } else if (param_name == "controller.slowdown_radius") {
         slowdown_radius_ = parameter.as_double();
-      } else if (param_name == "controller.deceleration_max") {
-        deceleration_max_ = parameter.as_double();
       } else if (param_name == "controller.rotate_to_heading_angular_vel") {
         rotate_to_heading_angular_vel_ = parameter.as_double();
       } else if (param_name == "controller.rotate_to_heading_max_angular_accel") {
@@ -283,7 +277,6 @@ Controller::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
       // Update the smooth control law with the new params
       control_law_->setCurvatureConstants(k_phi_, k_delta_, beta_, lambda_);
       control_law_->setSlowdownRadius(slowdown_radius_);
-      control_law_->setMaxDeceleration(deceleration_max_);
       control_law_->setSpeedLimit(v_linear_min_, v_linear_max_, v_angular_max_);
     }
   }
diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp
index 4ad66c45356..b5cedfa59bd 100644
--- a/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp
+++ b/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp
@@ -49,6 +49,22 @@ class SmoothControlLaw
     double k_phi, double k_delta, double beta, double lambda,
     double slowdown_radius, double deceleration_max,
     double v_linear_min, double v_linear_max, double v_angular_max);
+  
+  /**
+   * @brief Constructor for nav2_graceful_controller::SmoothControlLaw
+   *
+   * @param k_phi Ratio of the rate of change in phi to the rate of change in r.
+   * @param k_delta Constant factor applied to the heading error feedback.
+   * @param beta Constant factor applied to the path curvature: dropping velocity.
+   * @param lambda Constant factor applied to the path curvature for sharpness.
+   * @param slowdown_radius Radial threshold applied to the slowdown rule.
+   * @param v_linear_min Minimum linear velocity.
+   * @param v_linear_max Maximum linear velocity.
+   * @param v_angular_max Maximum angular velocity.
+   */
+  SmoothControlLaw(
+    double k_phi, double k_delta, double beta, double lambda, double slowdown_radius,
+    double v_linear_min, double v_linear_max, double v_angular_max);
 
   /**
    * @brief Destructor for nav2_graceful_controller::SmoothControlLaw
@@ -105,6 +121,20 @@ class SmoothControlLaw
     const double & target_distance,
     const bool & backward = false);
 
+  /**
+   * @brief Compute linear and angular velocities command using the curvature.
+   *
+   * @param target Pose of the target in the robot frame.
+   * @param current Current pose of the robot in the robot frame.
+   * @param target_distance Path distance from current to target frame.
+   * @param backward If true, the robot is moving backwards. Defaults to false.
+   * @return Velocity command.
+   */
+  geometry_msgs::msg::Twist calculateRegularVelocity(
+    const geometry_msgs::msg::Pose & target,
+    const geometry_msgs::msg::Pose & current,
+    const bool & backward = false);
+
   /**
    * @brief Compute linear and angular velocities command using the curvature.
    *
@@ -133,6 +163,23 @@ class SmoothControlLaw
     const double & target_distance,
     const bool & backward = false);
 
+  /**
+   * @brief Calculate the next pose of the robot by generating a velocity command using the
+   * curvature and the current pose.
+   *
+   * @param dt Time step.
+   * @param target Pose of the target in the robot frame.
+   * @param current Current pose of the robot in the robot frame.
+   * @param target_distance Path distance from current to target frame.
+   * @param backward If true, the robot is moving backwards. Defaults to false.
+   * @return geometry_msgs::msg::Pose
+   */
+  geometry_msgs::msg::Pose calculateNextPose(
+    const double dt,
+    const geometry_msgs::msg::Pose & target,
+    const geometry_msgs::msg::Pose & current,
+    const bool & backward = false);
+
 protected:
   /**
    * @brief Calculate the path curvature using a Lyapunov-based feedback control law from
diff --git a/nav2_graceful_controller/src/smooth_control_law.cpp b/nav2_graceful_controller/src/smooth_control_law.cpp
index 9092932d570..fdf6f671b5b 100644
--- a/nav2_graceful_controller/src/smooth_control_law.cpp
+++ b/nav2_graceful_controller/src/smooth_control_law.cpp
@@ -30,6 +30,14 @@ SmoothControlLaw::SmoothControlLaw(
 {
 }
 
+SmoothControlLaw::SmoothControlLaw(
+  double k_phi, double k_delta, double beta, double lambda, double slowdown_radius,
+  double v_linear_min, double v_linear_max, double v_angular_max)
+: k_phi_(k_phi), k_delta_(k_delta), beta_(beta), lambda_(lambda), slowdown_radius_(slowdown_radius),
+  v_linear_min_(v_linear_min), v_linear_max_(v_linear_max), v_angular_max_(v_angular_max)
+{
+}
+
 void SmoothControlLaw::setCurvatureConstants(
   double k_phi, double k_delta, double beta, double lambda)
 {
@@ -99,12 +107,49 @@ geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
   return cmd_vel;
 }
 
+geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
+  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
+  const bool & backward)
+{
+  // Convert the target to polar coordinates
+  auto ego_coords = EgocentricPolarCoordinates(target, current, backward);
+  // Calculate the curvature
+  double curvature = calculateCurvature(ego_coords.r, ego_coords.phi, ego_coords.delta);
+  // Invert the curvature if the robot is moving backwards
+  curvature = backward ? -curvature : curvature;
+
+  // Adjust the linear velocity as a function of the path curvature to
+  // slowdown the controller as it approaches its target
+  double v = v_linear_max_ / (1.0 + beta_ * std::pow(fabs(curvature), lambda_));
+
+  // Slowdown when the robot is near the target to remove singularity
+  v = std::min(v_linear_max_ * (ego_coords.r / slowdown_radius_), v);
+
+  // Set some small v_min when far away from origin to promote faster
+  // turning motion when the curvature is very high
+  v = std::clamp(v, v_linear_min_, v_linear_max_);
+
+  // Set the velocity to negative if the robot is moving backwards
+  v = backward ? -v : v;
+
+  // Compute the angular velocity
+  double w = curvature * v;
+  // Bound angular velocity between [-max_angular_vel, max_angular_vel]
+  double w_bound = std::clamp(w, -v_angular_max_, v_angular_max_);
+  // And linear velocity to follow the curvature
+  v = (curvature != 0.0) ? (w_bound / curvature) : v;
+
+  // Return the velocity command
+  geometry_msgs::msg::Twist cmd_vel;
+  cmd_vel.linear.x = v;
+  cmd_vel.angular.z = w_bound;
+  return cmd_vel;
+}
+
 geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
   const geometry_msgs::msg::Pose & target, const bool & backward)
 {
-  geometry_msgs::msg::Pose current_pose = geometry_msgs::msg::Pose();
-  double target_distance = nav2_util::geometry_utils::euclidean_distance(target, current_pose);
-  return calculateRegularVelocity(target, current_pose, target_distance, backward);
+  return calculateRegularVelocity(target, geometry_msgs::msg::Pose(), backward);
 }
 
 geometry_msgs::msg::Pose SmoothControlLaw::calculateNextPose(
@@ -125,6 +170,22 @@ geometry_msgs::msg::Pose SmoothControlLaw::calculateNextPose(
   return next;
 }
 
+geometry_msgs::msg::Pose SmoothControlLaw::calculateNextPose(
+  const double dt,
+  const geometry_msgs::msg::Pose & target,
+  const geometry_msgs::msg::Pose & current,
+  const bool & backward)
+{
+  geometry_msgs::msg::Twist vel = calculateRegularVelocity(target, current, backward);
+  geometry_msgs::msg::Pose next;
+  double yaw = tf2::getYaw(current.orientation);
+  next.position.x = current.position.x + vel.linear.x * dt * cos(yaw);
+  next.position.y = current.position.y + vel.linear.x * dt * sin(yaw);
+  yaw += vel.angular.z * dt;
+  next.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw);
+  return next;
+}
+
 double SmoothControlLaw::calculateCurvature(double r, double phi, double delta)
 {
   // Calculate the proportional term of the control law as the product of the gain and the error: