[RPP] Jerk limited trajectory generation

nav2_regulated_pure_pursuit_controller/CMakeLists.txt

@@ -11,6 +11,8 @@ find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(tf2 REQUIRED)
+find_package(ruckig REQUIRED)
+find_package(angles REQUIRED)
 
 nav2_package()
 set(CMAKE_CXX_STANDARD 17)
@@ -28,13 +30,15 @@ set(dependencies
   nav2_util
   nav2_core
   tf2
+  ruckig
+  angles
 )
 
 set(library_name nav2_regulated_pure_pursuit_controller)
 
 add_library(${library_name} SHARED
         src/regulated_pure_pursuit_controller.cpp)
-
+target_link_libraries(${library_name} ruckig::ruckig)
 ament_target_dependencies(${library_name}
   ${dependencies}
 )

nav2_regulated_pure_pursuit_controller/README.md

@@ -36,7 +36,7 @@ The Regulated Pure Pursuit controller implements active collision detection. We
 
 The regulated pure pursuit algorithm also makes use of the common variations on the pure pursuit algorithm. We implement the adaptive pure pursuit's main contribution of having velocity-scaled lookahead point distances. This helps make the controller more stable over a larger range of potential linear velocities. There are parameters for setting the lookahead gain (or lookahead time) and thresholded values for minimum and maximum.
 
-The final minor improvement we make is slowing on approach to the goal. Knowing that the optimal lookahead distance is `X`, we can take the difference in `X` and the actual distance of the lookahead point found to find the lookahead point error. During operations, the variation in this error should be exceptionally small and won't be triggered. However, at the end of the path, there are no more points at a lookahead distance away from the robot, so it uses the last point on the path. So as the robot approaches a target, its error will grow and the robot's velocity will be reduced proportional to this error until a minimum threshold. This is also tracked by the kinematic speed limits to ensure drivability.
+The final minor improvement we make is slowing on approach to the goal. Knowing the remaining path length we are able to calculate motion profile that brings us to stop on the goal point. In case of robots that are able to rotate in place, there is a proportional angle controller implemented in order to precisely track the angle setpoint.
 
 The major improvements that this work implements is the regulations on the linear velocity based on some cost functions.  They were selected to remove long-standing bad behavior within the pure pursuit algorithm. Normal Pure Pursuit has an issue with overshoot and poor handling in particularly high curvature (or extremely rapidly changing curvature) environments. It is commonly known that this will cause the robot to overshoot from the path and potentially collide with the environment. These cost functions in the Regulated Pure Pursuit algorithm were also chosen based on common requirements and needs of mobile robots uses in service, commercial, and industrial use-cases; scaling by curvature creates intuitive behavior of slowing the robot when making sharp turns and slowing when its near a potential collision so that small variations don't clip obstacles. This is also really useful when working in partially observable environments (like turning in and out of aisles / hallways often) so that you slow before a sharp turn into an unknown dynamic environment to be more conservative in case something is in the way immediately requiring a stop.
 
@@ -52,18 +52,19 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin
 
 | Parameter | Description | 
 |-----|----|
-| `desired_linear_vel` | The desired maximum linear velocity to use. | 
-| `max_linear_accel` | Acceleration for linear velocity | 
-| `max_linear_decel` | Deceleration for linear velocity | 
+| `desired_linear_vel` | The desired maximum linear velocity to use. |
+| `max_linear_accel` | Acceleration for linear velocity |
+| `max_linear_decel` | Deceleration for linear velocity |
+| `max_linear_jerk` | Jerk for linear velocity |
+| `max_angular_jerk` | Jerk for angular velocity |
+| `kp_angle` | Proportional regulator gain while rotating in place |
 | `lookahead_dist` | The lookahead distance to use to find the lookahead point | 
 | `min_lookahead_dist` | The minimum lookahead distance threshold when using velocity scaled lookahead distances | 
 | `max_lookahead_dist` | The maximum lookahead distance threshold when using velocity scaled lookahead distances | 
 | `lookahead_time` | The time to project the velocity by to find the velocity scaled lookahead distance. Also known as the lookahead gain. | 
 | `rotate_to_heading_angular_vel` | If rotate to heading is used, this is the angular velocity to use. | 
 | `transform_tolerance` | The TF transform tolerance | 
 | `use_velocity_scaled_lookahead_dist` | Whether to use the velocity scaled lookahead distances or constant `lookahead_distance` | 
-| `min_approach_linear_velocity` | The minimum velocity threshold to apply when approaching the goal | 
-| `use_approach_linear_velocity_scaling` | Whether to scale the linear velocity down on approach to the goal for a smooth stop | 
 | `max_allowed_time_to_collision_up_to_carrot` | The time to project a velocity command to check for collisions, limited to maximum distance of lookahead distance selected | 
 | `use_regulated_linear_velocity_scaling` | Whether to use the regulated features for curvature | 
 | `use_cost_regulated_linear_velocity_scaling` | Whether to use the regulated features for proximity to obstacles | 
@@ -106,15 +107,16 @@ controller_server:
       desired_linear_vel: 0.5
       max_linear_accel: 2.5
       max_linear_decel: 2.5
+      max_linear_jerk: 10000.0
+      max_angular_jerk: 10000.0
+      kp_angle: 3.0
       lookahead_dist: 0.6
       min_lookahead_dist: 0.3
       max_lookahead_dist: 0.9
       lookahead_time: 1.5
       rotate_to_heading_angular_vel: 1.8
       transform_tolerance: 0.1
       use_velocity_scaled_lookahead_dist: false
-      min_approach_linear_velocity: 0.05
-      use_approach_linear_velocity_scaling: true
       max_allowed_time_to_collision_up_to_carrot: 1.0
       use_regulated_linear_velocity_scaling: true
       use_cost_regulated_linear_velocity_scaling: false
@@ -150,3 +152,5 @@ To tune to get Pure Pursuit behaviors, set all boolean parameters to false and m
 Currently, there is no rotate to goal behaviors, so it is expected that the path approach orientations are the orientations of the goal or the goal checker has been set with a generous `min_theta_velocity_threshold`. Implementations for rotating to goal heading are on the way.
 
 The choice of lookahead distances are highly dependent on robot size, responsiveness, controller update rate, and speed. Please make sure to tune this for your platform, although the `regulated` features do largely make heavy tuning of this value unnecessary. If you see wiggling, increase the distance or scale. If it's not converging as fast to the path as you'd like, decrease it.
+
+Default jerk limits are set to high values. This essentially generates trapezoidal velocity profiles and fits most of users. Advanced users can change these values but should be aware that too low jerk limits can lead to poor path tracking and oscillations.

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -30,6 +30,7 @@
 #include "nav2_util/odometry_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "geometry_msgs/msg/pose2_d.hpp"
+#include "ruckig/ruckig.hpp"
 
 namespace nav2_regulated_pure_pursuit_controller
 {
@@ -160,9 +161,11 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   /**
    * @brief Whether robot should rotate to final goal orientation
    * @param carrot_pose current lookahead point
+   * @param lookahead_dist current lookahead distance
    * @return Whether should rotate to goal heading
    */
-  bool shouldRotateToGoalHeading(const geometry_msgs::msg::PoseStamped & carrot_pose);
+  bool shouldRotateToGoalHeading(
+    const geometry_msgs::msg::PoseStamped & carrot_pose, const double & lookahead_dist);
 
   /**
    * @brief Create a smooth and kinematically smoothed rotation command
@@ -240,9 +243,11 @@ class RegulatedPurePursuitController : public nav2_core::Controller
    * @brief Get lookahead point
    * @param lookahead_dist Optimal lookahead distance
    * @param path Current global path
+   * @param remaining_path_length path length from lookahead point to the end of pruned path
    * @return Lookahead point
    */
-  geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &);
+  geometry_msgs::msg::PoseStamped getLookAheadPoint(
+    const double &, const nav_msgs::msg::Path &, double & remaining_path_length);
 
   /**
    * @brief checks for the cusp position
@@ -273,14 +278,15 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   rclcpp::Clock::SharedPtr clock_;
 
   double desired_linear_vel_, base_desired_linear_vel_;
+  double max_linear_accel_;
+  double max_linear_decel_;
   double lookahead_dist_;
   double rotate_to_heading_angular_vel_;
   double max_lookahead_dist_;
   double min_lookahead_dist_;
   double lookahead_time_;
   bool use_velocity_scaled_lookahead_dist_;
   tf2::Duration transform_tolerance_;
-  double min_approach_linear_velocity_;
   double control_duration_;
   double max_allowed_time_to_collision_up_to_carrot_;
   bool use_regulated_linear_velocity_scaling_;
@@ -295,6 +301,11 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   double rotate_to_heading_min_angle_;
   double goal_dist_tol_;
   bool allow_reversing_;
+  double robot_angle_;
+  double kp_angle_;
+  double max_linear_jerk_;
+  double max_angular_jerk_;
+  rclcpp::Time system_time_;
   double max_robot_pose_search_dist_;
   bool use_interpolation_;
 
@@ -306,6 +317,16 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   std::unique_ptr<nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>
   collision_checker_;
 
+  std::unique_ptr<ruckig::Ruckig<1>> distance_profile_;
+  ruckig::InputParameter<1> distance_profile_input_;
+  ruckig::OutputParameter<1> distance_profile_output_;
+  ruckig::Result distance_profile_result_;
+
+  std::unique_ptr<ruckig::Ruckig<1>> angle_profile_;
+  ruckig::InputParameter<1> angle_profile_input_;
+  ruckig::OutputParameter<1> angle_profile_output_;
+  ruckig::Result angle_profile_result_;
+
   // Dynamic parameters handler
   std::mutex mutex_;
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;

nav2_regulated_pure_pursuit_controller/package.xml

@@ -19,6 +19,8 @@
   <depend>nav2_msgs</depend>
   <depend>pluginlib</depend>
   <depend>tf2</depend>
+  <depend>ruckig</depend>
+  <depend>angles</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_common</test_depend>