smoothing: add jerk limitation

Author: baumanta

local_planner/cfg/local_planner_node.cfg

@@ -36,6 +36,8 @@ gen.add("reproj_age_", int_t, 0, "maximum age of a reprojected point", 50, 0, 10
 gen.add("relevance_margin_e_degree_", double_t, 0, "obstacles at more than this angle from goal direction are ignored", 25, 0, 90)
 gen.add("relevance_margin_z_degree_", double_t, 0, "obstacles at more than this angle from goal direction are ignored", 40, 0, 180)
 gen.add("velocity_sigmoid_slope_", double_t, 0, "the bigger the bigger the acceleration", 1, 0, 10)
+gen.add("max_jerk_limit_", double_t, 0, "max jerk for smoothing (set to 0 to disable)", 700, 0, 2000)
+gen.add("min_jerk_limit_", double_t, 0, "min jerk for velocity-dependent max jerk (set to 0 to disable velocity-dependence)", 200, 0, 1000)
 
 gen.add("use_vel_setpoints_", bool_t, 0, "Enable velocity setpoints (if false, position setpoints are used)", False)
 gen.add("stop_in_front_", bool_t, 0, "Enable stop in front of the obstacle", False)

local_planner/src/nodes/local_planner_node.cpp

@@ -878,6 +878,8 @@ void LocalPlannerNode::dynamicReconfigureCallback(
     avoidance::LocalPlannerNodeConfig &config, uint32_t level) {
   std::lock_guard<std::mutex> guard(running_mutex_);
   local_planner_.dynamicReconfigureSetParams(config, level);
+  wp_generator_.setMinJerkLimit(config.min_jerk_limit_);
+  wp_generator_.setMaxJerkLimit(config.max_jerk_limit_);
 }
 
 void LocalPlannerNode::threadFunction() {

local_planner/src/nodes/waypoint_generator.cpp

@@ -244,33 +244,31 @@ void WaypointGenerator::reachGoalAltitudeFirst() {
   output_.goto_position.z = pose_.pose.position.z + pose_to_wp.z;
 }
 
-// smooth trajectory by liming the maximim accelleration possible
-void WaypointGenerator::smoothWaypoint() {
-  ros::Time time = ros::Time::now();
-  ros::Duration time_diff = time - last_t_smooth_;
-  double dt = time_diff.toSec() > 0.0 ? time_diff.toSec() : 0.004;
-  last_t_smooth_ = time;
+void WaypointGenerator::smoothWaypoint(double dt) {
 
-  Eigen::Vector2f vel_xy(curr_vel_.twist.linear.x, curr_vel_.twist.linear.y);
-  Eigen::Vector2f vel_waypt_xy(
+  Eigen::Vector2f vel_cur_xy(curr_vel_.twist.linear.x, curr_vel_.twist.linear.y);
+  Eigen::Vector2f vel_sp_xy(
       (output_.adapted_goto_position.x - last_position_waypoint_.pose.position.x) / dt,
       (output_.adapted_goto_position.y - last_position_waypoint_.pose.position.y) / dt);
-  Eigen::Vector2f vel_waypt_xy_prev(
-      (last_position_waypoint_.pose.position.x -
-       last_last_position_waypoint_.pose.position.x) /
-          dt,
-      (last_position_waypoint_.pose.position.y -
-       last_last_position_waypoint_.pose.position.y) /
-          dt);
-  Eigen::Vector2f acc_waypt_xy((vel_waypt_xy - vel_waypt_xy_prev) / dt);
-
-  if (acc_waypt_xy.norm() > (acc_waypt_xy.norm() / 2.0f)) {
-    vel_xy = (acc_waypt_xy.norm() / 2.0f) * acc_waypt_xy.normalized() * dt +
-             vel_waypt_xy_prev;
+
+  Eigen::Vector2f accel_diff = (vel_sp_xy - vel_cur_xy) / dt;
+  Eigen::Vector2f accel_cur = (vel_cur_xy - last_velocity_) / dt;
+  Eigen::Vector2f jerk_diff = (accel_diff - accel_cur) / dt;
+  float max_jerk = max_jerk_limit_param_;
+
+  // velocity-dependent max jerk
+  if (min_jerk_limit_param_ > 0.001f) {
+    max_jerk *= vel_cur_xy.norm();
+    if (max_jerk < min_jerk_limit_param_) max_jerk = min_jerk_limit_param_;
+  }
+
+  if (jerk_diff.squaredNorm() > max_jerk * max_jerk && max_jerk > 0.001f) {
+    jerk_diff = max_jerk * jerk_diff.normalized();
+    vel_sp_xy = (jerk_diff * dt + accel_cur) * dt + vel_cur_xy;
   }
 
-  output_.smoothed_goto_position.x = last_position_waypoint_.pose.position.x + vel_xy(0) * dt;
-  output_.smoothed_goto_position.y = last_position_waypoint_.pose.position.y + vel_xy(1) * dt;
+  output_.smoothed_goto_position.x = last_position_waypoint_.pose.position.x + vel_sp_xy(0) * dt;
+  output_.smoothed_goto_position.y = last_position_waypoint_.pose.position.y + vel_sp_xy(1) * dt;
   output_.smoothed_goto_position.z = output_.adapted_goto_position.z;
 
   ROS_DEBUG("[WG] Smoothed waypoint: [%f %f %f].", output_.smoothed_goto_position.x,
@@ -354,11 +352,12 @@ void WaypointGenerator::adaptSpeed() {
 // create the message that is sent to the UAV
 void WaypointGenerator::getPathMsg() {
   output_.path.header.frame_id = "/world";
-  last_last_position_waypoint_ = last_position_waypoint_;
-  last_position_waypoint_ = output_.position_waypoint;
-  last_yaw_ = curr_yaw_;
   output_.adapted_goto_position = output_.goto_position;
 
+  const ros::Time now = ros::Time::now();
+  ros::Duration time_diff = now - last_time_;
+  double dt = time_diff.toSec() > 0.0 ? time_diff.toSec() : 0.004;
+
   // If avoid sphere is used, project waypoint on sphere
   if (planner_info_.use_avoid_sphere && planner_info_.avoid_sphere_age < 100 &&
 		  planner_info_.reach_altitude && !reached_goal_) {
@@ -387,11 +386,7 @@ void WaypointGenerator::getPathMsg() {
     ROS_DEBUG("[WG] pose altitude func: [%f %f %f].", pose_.pose.position.x,
               pose_.pose.position.y, pose_.pose.position.z);
   } else {
-    if (!only_yawed_ && !reached_goal_) {
-      smoothWaypoint();
-      new_yaw_ = nextYaw(pose_, output_.smoothed_goto_position);
-      ;
-    }
+    smoothWaypoint(dt);
   }
 
   // change waypoint if drone is at goal or above
@@ -431,10 +426,10 @@ void WaypointGenerator::getPathMsg() {
   transformPositionToVelocityWaypoint();
 
   output_.path.poses.push_back(output_.position_waypoint);
-  curr_yaw_ = new_yaw_;
-  last_last_position_waypoint_ = last_position_waypoint_;
   last_position_waypoint_ = output_.position_waypoint;
   last_yaw_ = curr_yaw_;
+  last_velocity_ = Eigen::Vector2f(curr_vel_.twist.linear.x, curr_vel_.twist.linear.y);
+  last_time_ = now;
 }
 
 void WaypointGenerator::getWaypoints(waypointResult &output) {

local_planner/src/nodes/waypoint_generator.h

@@ -47,6 +47,10 @@ class WaypointGenerator {
   double curr_vel_magnitude_;
   ros::Time update_time_;
   geometry_msgs::TwistStamped curr_vel_;
+  ros::Time last_time_{0.};
+
+  double max_jerk_limit_param_{500.};
+  double min_jerk_limit_param_{200.};
 
   bool reached_goal_;
   bool waypoint_outside_FOV_;
@@ -59,11 +63,10 @@ class WaypointGenerator {
 
   geometry_msgs::Point hover_position_;
   geometry_msgs::PoseStamped last_position_waypoint_;
-  geometry_msgs::PoseStamped last_last_position_waypoint_;
+  Eigen::Vector2f last_velocity_{0.f, 0.f}; ///< last vehicle's velocity
 
   ros::Time velocity_time_;
   std::vector<int> z_FOV_idx_;
-  ros::Time last_t_smooth_;
 
   void calculateWaypoint();
   void updateState();
@@ -72,17 +75,34 @@ class WaypointGenerator {
   void transformPositionToVelocityWaypoint();
   bool withinGoalRadius();
   void reachGoalAltitudeFirst();
-  void smoothWaypoint();
+  void smoothWaypoint(double dt);
   void adaptSpeed();
   void getPathMsg();
 
  public:
+
   void getWaypoints(waypointResult &output);
   void setPlannerInfo(avoidanceOutput input);
   void updateState(geometry_msgs::PoseStamped act_pose,
                    geometry_msgs::PoseStamped goal,
                    geometry_msgs::TwistStamped vel, bool stay, ros::Time t);
 
+  /**
+   * Set maximum jerk limitation. Set to 0 to disable.
+   */
+  void setMaxJerkLimit(double max_jerk_limit) {
+    max_jerk_limit_param_ = max_jerk_limit;
+  }
+
+  /**
+   * Set minimum jerk limitation for velocity-depdendent jerk limit.
+   * Set to 0 to disable velocity-dependent jerk limit, and use a fixed
+   * limit instead.
+   */
+  void setMinJerkLimit(double min_jerk_limit) {
+    min_jerk_limit_param_ = min_jerk_limit;
+  }
+
   WaypointGenerator();
   ~WaypointGenerator();
 };