add max vel linear

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",
@@ -327,6 +331,12 @@ 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)
 
+# Optimization/Omni
+grp_optimization_omni = grp_optimization.add_group("OptOmni", type="hide")
+
+grp_optimization_omni.add("norm_vel_trans", int_t, 0,
+	"Norm constraint for translational velocity (1: L1, 2: L2). For example, enforcing hypot(vx, xy) <= vel_max_trans for L2",
+	1, 1, 2)
 
 
 # Homotopy Class Planner

include/teb_local_planner/g2o_types/edge_velocity.h

@@ -251,9 +251,20 @@ 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();
-
+
+    double max_vel_trans = std::max(std::max(cfg_->robot.max_vel_x, cfg_->robot.max_vel_y), cfg_->robot.max_vel_trans);
+
+    double max_vel_trans_remaining;
+    if (cfg_->optim.norm_vel_trans == 1) {
+      max_vel_trans_remaining = max_vel_trans - std::abs(vx);  // L1 norm
+    }
+    else {
+      max_vel_trans_remaining = std::sqrt(max_vel_trans * max_vel_trans - vx * vx); // L2 norm
+    }
+    double max_vel_y = std::min(max_vel_trans_remaining, cfg_->robot.max_vel_y);
+
     _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
+    _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
@@ -172,6 +173,7 @@ class TebConfig
 
     double weight_adapt_factor; //!< Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new = weight_old*factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.
     double obstacle_cost_exponent; //!< Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)
+    double norm_vel_trans; //!< Norm constraint for translational velocity (1: L1, 2: L2)
   } optim; //!< Optimization related parameters
 
 
@@ -270,6 +272,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;
@@ -336,6 +339,7 @@ class TebConfig
 
     optim.weight_adapt_factor = 2.0;
     optim.obstacle_cost_exponent = 1.0;
+    optim.norm_vel_trans = 1;
 
     // Homotopy Class Planner
 

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);
@@ -136,6 +137,7 @@ void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh)
   nh.param("weight_prefer_rotdir", optim.weight_prefer_rotdir, optim.weight_prefer_rotdir);
   nh.param("weight_adapt_factor", optim.weight_adapt_factor, optim.weight_adapt_factor);
   nh.param("obstacle_cost_exponent", optim.obstacle_cost_exponent, optim.obstacle_cost_exponent);
+  nh.param("norm_vel_trans", optim.norm_vel_trans, optim.norm_vel_trans);
 
   // Homotopy Class Planner
   nh.param("enable_homotopy_class_planning", hcp.enable_homotopy_class_planning, hcp.enable_homotopy_class_planning); 
@@ -213,6 +215,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;
@@ -261,6 +264,7 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg)
   optim.weight_viapoint = cfg.weight_viapoint;
   optim.weight_adapt_factor = cfg.weight_adapt_factor;
   optim.obstacle_cost_exponent = cfg.obstacle_cost_exponent;
+  optim.norm_vel_trans = cfg.norm_vel_trans;
 
   // Homotopy Class Planner
   hcp.enable_multithreading = cfg.enable_multithreading;
@@ -350,6 +354,13 @@ 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 < robot.max_vel_x || robot.max_vel_trans < robot.max_vel_y) {
+      ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_trans < max_vel_x or max_vel_trans < max_vel_y.");
+    }
+  }
 
 }    
 

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;
+  }
 }