use separate decel_lim_x for control law (#45) (#49)

mikeferguson · Jun 18, 2022 · ee2b2d8 · ee2b2d8
1 parent 1f771f6
commit ee2b2d8
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 8 deletions.
diff --git a/graceful_controller/include/graceful_controller/graceful_controller.hpp b/graceful_controller/include/graceful_controller/graceful_controller.hpp
@@ -1,5 +1,5 @@
 /*
- * Copyright 2021 Michael Ferguson
+ * Copyright 2021-2022 Michael Ferguson
  * Copyright 2015 Fetch Robotics Inc
  * Author: Michael Ferguson
  *
@@ -35,14 +35,14 @@ class GracefulController
    * @param k2 How quickly we converge to the slow manifold.
    * @param min_velocity The minimum velocity in the linear direction.
    * @param max_velocity The maximum velocity in the linear direction.
-   * @param max_accel The maximum acceleration in the linear direction.
+   * @param max_decel The maximum deceleration in the linear direction.
    * @param max_angular_velocity The maximum velocity in the linear direction.
    */
   GracefulController(double k1,
                      double k2,
                      double min_velocity,
                      double max_velocity,
-                     double max_accel,
+                     double max_decel,
                      double max_angular_velocity,
                      double beta,
                      double lambda);
@@ -79,7 +79,7 @@ class GracefulController
   double k2_;  // speed at which we converge to slow system
   double min_velocity_;
   double max_velocity_;
-  double max_accel_;
+  double max_decel_;
   double max_angular_velocity_;
   double beta_;  // how fast velocity drops as k increases
   double lambda_;  // ??

diff --git a/graceful_controller/src/graceful_controller.cpp b/graceful_controller/src/graceful_controller.cpp
@@ -31,15 +31,15 @@ namespace graceful_controller
 
 GracefulController::GracefulController(double k1, double k2,
                                        double min_velocity, double max_velocity,
-                                       double max_accel,
+                                       double max_decel,
                                        double max_angular_velocity,
                                        double beta, double lambda)
 {
   k1_ = k1;
   k2_ = k2;
   min_velocity_ = min_velocity;
   max_velocity_ = max_velocity;
-  max_accel_ = max_accel;
+  max_decel_ = max_decel;
   max_angular_velocity_ = max_angular_velocity;
   beta_ = beta;
   lambda_ = lambda;
@@ -66,7 +66,7 @@ bool GracefulController::approach(const double x, const double y, const double t
   // Compute max_velocity based on curvature
   double v = max_velocity_ / (1 + beta_ * std::pow(fabs(k), lambda_));
   // Limit velocity based on approaching target
-  double approach_limit = std::sqrt(2 * max_accel_ * r);
+  double approach_limit = std::sqrt(2 * max_decel_ * r);
   v = std::min(v, approach_limit);
   v = std::min(std::max(v, min_velocity_), max_velocity_);
 

diff --git a/graceful_controller_ros/include/graceful_controller_ros/graceful_controller_ros.hpp b/graceful_controller_ros/include/graceful_controller_ros/graceful_controller_ros.hpp
@@ -165,6 +165,7 @@ class GracefulControllerROS : public nav2_core::Controller
   double min_in_place_vel_theta_;
   double acc_lim_x_;
   double acc_lim_theta_;
+  double decel_lim_x_;
   double scaling_vel_x_;
   double scaling_factor_;
   double scaling_step_;

diff --git a/graceful_controller_ros/src/graceful_controller_ros.cpp b/graceful_controller_ros/src/graceful_controller_ros.cpp
@@ -191,6 +191,7 @@ void GracefulControllerROS::configure(
   declare_parameter_if_not_declared(node, name_ + ".acc_lim_x", rclcpp::ParameterValue(2.5));
   declare_parameter_if_not_declared(node, name_ + ".acc_lim_theta", rclcpp::ParameterValue(3.2));
   declare_parameter_if_not_declared(node, name_ + ".acc_dt", rclcpp::ParameterValue(0.25));
+  declare_parameter_if_not_declared(node, name_ + ".decel_lim_x", rclcpp::ParameterValue(0.0));
   declare_parameter_if_not_declared(node, name_ + ".max_lookahead", rclcpp::ParameterValue(1.0));
   declare_parameter_if_not_declared(node, name_ + ".min_lookahead", rclcpp::ParameterValue(0.25));
   declare_parameter_if_not_declared(node, name_ + ".initial_rotate_tolerance", rclcpp::ParameterValue(0.1));
@@ -216,6 +217,7 @@ void GracefulControllerROS::configure(
   node->get_parameter(name_ + ".acc_lim_x", acc_lim_x_);
   node->get_parameter(name_ + ".acc_lim_theta", acc_lim_theta_);
   node->get_parameter(name_ + ".acc_dt", acc_dt_);
+  node->get_parameter(name_ + ".decel_lim_x", decel_lim_x_);
   node->get_parameter(name_ + ".max_lookahead", max_lookahead_);
   node->get_parameter(name_ + ".min_lookahead", min_lookahead_);
   node->get_parameter(name_ + ".initial_rotate_tolerance", initial_rotate_tolerance_);
@@ -251,11 +253,17 @@ void GracefulControllerROS::configure(
     collision_points_ = new visualization_msgs::msg::MarkerArray();
   }
 
+  if (decel_lim_x_ < 0.001)
+  {
+    // If decel limit not specified, use accel_limit
+    decel_lim_x_ = acc_lim_x_;
+  }
+
   controller_ = std::make_shared<GracefulController>(k1,
                                                      k2,
                                                      min_vel_x_,
                                                      max_vel_x_,
-                                                     acc_lim_x_,
+                                                     decel_lim_x_,
                                                      max_vel_theta_,
                                                      beta,
                                                      lambda);