Add max vel linear to limit the maximum linear velocity on holonomic …

…robots

cfg/TebLocalPlannerReconfigure.cfg

@@ -141,7 +141,11 @@ grp_robot_omni = grp_robot.add_group("Omnidirectional", type="hide")
 
 grp_robot_omni.add("max_vel_y", double_t, 0, 
   "Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)",
-  0.0, 0.0, 100) 
+  0.0, 0.0, 100)
+
+grp_robot_omni.add("max_vel_trans", double_t, 0, 
+  "Maximum linear velocity of the robot. Ignore this parameter for non-holonomic robots (max_vel_trans == max_vel_x).",
+  0.4, 0.01, 100)
 
 grp_robot_omni.add("acc_lim_y", double_t, 0, 
   "Maximum strafing acceleration of the robot",
@@ -326,7 +330,6 @@ grp_optimization.add("weight_adapt_factor", double_t, 0,
 grp_optimization.add("obstacle_cost_exponent", double_t, 0,
 	"Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)",
 	1, 0.01, 100)
-
 
 
 # Homotopy Class Planner

include/teb_local_planner/g2o_types/edge_velocity.h

@@ -251,9 +251,19 @@ class EdgeVelocityHolonomic : public BaseTebMultiEdge<3, double>
     double vx = r_dx / deltaT->estimate();
     double vy = r_dy / deltaT->estimate();
     double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) / deltaT->estimate();
-
-    _error[0] = penaltyBoundToInterval(vx, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x, cfg_->optim.penalty_epsilon);
-    _error[1] = penaltyBoundToInterval(vy, cfg_->robot.max_vel_y, 0.0); // we do not apply the penalty epsilon here, since the velocity could be close to zero
+
+
+    double max_vel_trans_remaining_y;
+    double max_vel_trans_remaining_x;
+    max_vel_trans_remaining_y = std::sqrt(std::max(0.0, cfg_->robot.max_vel_trans * cfg_->robot.max_vel_trans - vx * vx)); 
+    max_vel_trans_remaining_x = std::sqrt(std::max(0.0, cfg_->robot.max_vel_trans * cfg_->robot.max_vel_trans - vy * vy)); 
+
+    double max_vel_y = std::min(max_vel_trans_remaining_y, cfg_->robot.max_vel_y);
+    double max_vel_x = std::min(max_vel_trans_remaining_x, cfg_->robot.max_vel_x);
+    double max_vel_x_backwards = std::min(max_vel_trans_remaining_x, cfg_->robot.max_vel_x_backwards);
+
+    _error[0] = penaltyBoundToInterval(vx, -max_vel_x_backwards, max_vel_x, cfg_->optim.penalty_epsilon);
+    _error[1] = penaltyBoundToInterval(vy, max_vel_y, 0.0); // we do not apply the penalty epsilon here, since the velocity could be close to zero
     _error[2] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
 
     ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]) && std::isfinite(_error[2]),

include/teb_local_planner/teb_config.h

@@ -95,6 +95,7 @@ class TebConfig
     double max_vel_x; //!< Maximum translational velocity of the robot
     double max_vel_x_backwards; //!< Maximum translational velocity of the robot for driving backwards
     double max_vel_y; //!< Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)
+    double max_vel_trans; //!< Maximum translational velocity of the robot for omni robots, which is different from max_vel_x
     double max_vel_theta; //!< Maximum angular velocity of the robot
     double acc_lim_x; //!< Maximum translational acceleration of the robot
     double acc_lim_y; //!< Maximum strafing acceleration of the robot
@@ -270,6 +271,7 @@ class TebConfig
     robot.max_vel_x = 0.4;
     robot.max_vel_x_backwards = 0.2;
     robot.max_vel_y = 0.0;
+    robot.max_vel_trans = 0.4;
     robot.max_vel_theta = 0.3;
     robot.acc_lim_x = 0.5;
     robot.acc_lim_y = 0.5;

include/teb_local_planner/teb_local_planner_ros.h

@@ -352,11 +352,12 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
    * @param[in,out] omega The angular velocity that should be saturated.
    * @param max_vel_x Maximum translational velocity for forward driving
    * @param max_vel_y Maximum strafing velocity (for holonomic robots)
+   * @param max_vel_trans Maximum translational velocity for holonomic robots
    * @param max_vel_theta Maximum (absolute) angular velocity
    * @param max_vel_x_backwards Maximum translational velocity for backwards driving
    */
   void saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y,
-                        double max_vel_theta, double max_vel_x_backwards) const;
+                        double max_vel_trans, double max_vel_theta, double max_vel_x_backwards) const;
 
 
   /**

src/teb_config.cpp

@@ -74,6 +74,7 @@ void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh)
   nh.param("max_vel_x", robot.max_vel_x, robot.max_vel_x);
   nh.param("max_vel_x_backwards", robot.max_vel_x_backwards, robot.max_vel_x_backwards);
   nh.param("max_vel_y", robot.max_vel_y, robot.max_vel_y);
+  nh.param("max_vel_trans", robot.max_vel_trans, robot.max_vel_trans);
   nh.param("max_vel_theta", robot.max_vel_theta, robot.max_vel_theta);
   nh.param("acc_lim_x", robot.acc_lim_x, robot.acc_lim_x);
   nh.param("acc_lim_y", robot.acc_lim_y, robot.acc_lim_y);
@@ -213,6 +214,7 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg)
   robot.wheelbase = cfg.wheelbase;
   robot.cmd_angle_instead_rotvel = cfg.cmd_angle_instead_rotvel;
   robot.use_proportional_saturation = cfg.use_proportional_saturation;
+  robot.max_vel_trans = cfg.max_vel_trans;
 
   // GoalTolerance
   goal_tolerance.xy_goal_tolerance = cfg.xy_goal_tolerance;
@@ -350,6 +352,17 @@ void TebConfig::checkParameters() const
   // weights
   if (optim.weight_optimaltime <= 0)
       ROS_WARN("TebLocalPlannerROS() Param Warning: parameter weight_optimaltime shoud be > 0 (even if weight_shortest_path is in use)");
+
+  // holonomic check
+  if (robot.max_vel_y > 0) {
+    if (robot.max_vel_trans < std::min(robot.max_vel_x, robot.max_vel_trans)) {
+      ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_trans < min(max_vel_x, max_vel_y). Note that vel_trans = sqrt(Vx^2 + Vy^2), thus max_vel_trans will limit Vx and Vy in the optimization step.");
+    }
+
+    if (robot.max_vel_trans > std::max(robot.max_vel_x, robot.max_vel_y)) {
+      ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_trans > max(max_vel_x, max_vel_y). Robot will rotate and move diagonally to achieve max resultant vel (possibly max vel on both axis), limited by the max_vel_trans.");
+    }
+  }
 
 }    
 

src/teb_local_planner_ros.cpp

@@ -421,7 +421,8 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt
 
   // Saturate velocity, if the optimization results violates the constraints (could be possible due to soft constraints).
   saturateVelocity(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z,
-                   cfg_.robot.max_vel_x, cfg_.robot.max_vel_y, cfg_.robot.max_vel_theta, cfg_.robot.max_vel_x_backwards);
+                   cfg_.robot.max_vel_x, cfg_.robot.max_vel_y, cfg_.robot.max_vel_trans, cfg_.robot.max_vel_theta, 
+                   cfg_.robot.max_vel_x_backwards);
 
   // convert rot-vel to steering angle if desired (carlike robot).
   // The min_turning_radius is allowed to be slighly smaller since it is a soft-constraint
@@ -867,7 +868,8 @@ double TebLocalPlannerROS::estimateLocalGoalOrientation(const std::vector<geomet
 }
 
 
-void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y, double max_vel_theta, double max_vel_x_backwards) const
+void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y, double max_vel_trans, double max_vel_theta, 
+              double max_vel_x_backwards) const
 {
   double ratio_x = 1, ratio_omega = 1, ratio_y = 1;
   // Limit translational velocity for forward driving
@@ -903,6 +905,14 @@ void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega,
     vy *= ratio_y;
     omega *= ratio_omega;
   }
+
+  double vel_linear = std::hypot(vx, vy);
+  if (vel_linear > max_vel_trans)
+  {
+    double max_vel_trans_ratio = max_vel_trans / vel_linear;
+    vx *= max_vel_trans_ratio;
+    vy *= max_vel_trans_ratio;
+  }
 }