Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add max vel linear #346

Merged
merged 10 commits into from
Feb 8, 2022
Merged

add max vel linear #346

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
43fcced
add max vel linear
renan028 Jan 11, 2022
bd17a95
add norm vel linear parameter
renan028 Jan 12, 2022
afc4782
fix max vel linear order and default value
renan028 Jan 12, 2022
292b79c
change to use hypot and fix holonomic check
renan028 Jan 12, 2022
766d63c
rename linear->trans
renan028 Jan 12, 2022
d576ad1
remove clamp from max_vel_trans wrt max_vel_x and y
renan028 Jan 12, 2022
0a6a1e4
remove L1 and fix optimization
renan028 Jan 12, 2022
56e2d02
improve warning
renan028 Jan 19, 2022
16f1cc5
change to use std min
renan028 Jan 20, 2022
2662bf7
change "or" to "min" in warning
renan028 Jan 20, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions cfg/TebLocalPlannerReconfigure.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -143,8 +143,8 @@ 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)

grp_robot_omni.add("max_vel_linear", double_t, 0,
"Maximum linear velocity of the robot. Ignore this parameter for non-holonomic robots (max_vel_linear == max_vel_x).",
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,
Expand Down Expand Up @@ -334,8 +334,8 @@ grp_optimization.add("obstacle_cost_exponent", double_t, 0,
# Optimization/Omni
grp_optimization_omni = grp_optimization.add_group("OptOmni", type="hide")

grp_optimization_omni.add("norm_vel_lin", int_t, 0,
"Norm constraint for translational velocity (1: L1, 2: L2). For example, enforcing hypot(vx, xy) <= v_max_lin for L2",
grp_optimization_omni.add("norm_vel_trans", int_t, 0,
renan028 marked this conversation as resolved.
Show resolved Hide resolved
"Norm constraint for translational velocity (1: L1, 2: L2). For example, enforcing hypot(vx, xy) <= vel_max_trans for L2",
1, 1, 2)


Expand Down
12 changes: 6 additions & 6 deletions include/teb_local_planner/g2o_types/edge_velocity.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -252,16 +252,16 @@ class EdgeVelocityHolonomic : public BaseTebMultiEdge<3, double>
double vy = r_dy / deltaT->estimate();
double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) / deltaT->estimate();

double max_vel_linear = std::max(std::max(cfg_->robot.max_vel_x, cfg_->robot.max_vel_y), cfg_->robot.max_vel_linear);
double max_vel_trans = std::max(std::max(cfg_->robot.max_vel_x, cfg_->robot.max_vel_y), cfg_->robot.max_vel_trans);
renan028 marked this conversation as resolved.
Show resolved Hide resolved

double max_linear_vel_remaining;
if (cfg_->optim.norm_vel_lin == 1) {
max_linear_vel_remaining = max_vel_linear - std::abs(vx); // L1 norm
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_linear_vel_remaining = std::sqrt(max_vel_linear * max_vel_linear - vx * vx); // L2 norm
max_vel_trans_remaining = std::sqrt(max_vel_trans * max_vel_trans - vx * vx); // L2 norm
}
double max_vel_y = std::min(max_linear_vel_remaining, cfg_->robot.max_vel_y);
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, max_vel_y, 0.0); // we do not apply the penalty epsilon here, since the velocity could be close to zero
Expand Down
8 changes: 4 additions & 4 deletions include/teb_local_planner/teb_config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,7 +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_linear; //!< Maximum translational velocity of the robot for omni robots, which is different from max_vel_x
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
Expand Down Expand Up @@ -173,7 +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_lin; //!< Norm constraint for translational velocity (1: L1, 2: L2)
double norm_vel_trans; //!< Norm constraint for translational velocity (1: L1, 2: L2)
} optim; //!< Optimization related parameters


Expand Down Expand Up @@ -272,7 +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_linear = 0.4;
robot.max_vel_trans = 0.4;
robot.max_vel_theta = 0.3;
robot.acc_lim_x = 0.5;
robot.acc_lim_y = 0.5;
Expand Down Expand Up @@ -339,7 +339,7 @@ class TebConfig

optim.weight_adapt_factor = 2.0;
optim.obstacle_cost_exponent = 1.0;
optim.norm_vel_lin = 1;
optim.norm_vel_trans = 1;

// Homotopy Class Planner

Expand Down
4 changes: 2 additions & 2 deletions include/teb_local_planner/teb_local_planner_ros.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -352,12 +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_linear Maximum translational 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_linear, double max_vel_theta, double max_vel_x_backwards) const;
double max_vel_trans, double max_vel_theta, double max_vel_x_backwards) const;


/**
Expand Down
12 changes: 6 additions & 6 deletions src/teb_config.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +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_linear", robot.max_vel_linear, robot.max_vel_linear);
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);
Expand Down Expand Up @@ -137,7 +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_lin", optim.norm_vel_lin, optim.norm_vel_lin);
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);
Expand Down Expand Up @@ -215,7 +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_linear = cfg.max_vel_linear;
robot.max_vel_trans = cfg.max_vel_trans;

// GoalTolerance
goal_tolerance.xy_goal_tolerance = cfg.xy_goal_tolerance;
Expand Down Expand Up @@ -264,7 +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_lin = cfg.norm_vel_lin;
optim.norm_vel_trans = cfg.norm_vel_trans;

// Homotopy Class Planner
hcp.enable_multithreading = cfg.enable_multithreading;
Expand Down Expand Up @@ -357,8 +357,8 @@ void TebConfig::checkParameters() const

// holonomic check
if (robot.max_vel_y > 0) {
if (robot.max_vel_linear < robot.max_vel_x || robot.max_vel_linear < robot.max_vel_y) {
ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_linear < max_vel_x or max_vel_linear < max_vel_y.");
if (robot.max_vel_trans < robot.max_vel_x || robot.max_vel_trans < robot.max_vel_y) {
renan028 marked this conversation as resolved.
Show resolved Hide resolved
ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_trans < max_vel_x or max_vel_trans < max_vel_y.");
renan028 marked this conversation as resolved.
Show resolved Hide resolved
}
}
renan028 marked this conversation as resolved.
Show resolved Hide resolved

Expand Down
12 changes: 6 additions & 6 deletions src/teb_local_planner_ros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -421,7 +421,7 @@ 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_linear, cfg_.robot.max_vel_theta,
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).
Expand Down Expand Up @@ -868,7 +868,7 @@ 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_linear, double max_vel_theta,
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;
Expand Down Expand Up @@ -907,11 +907,11 @@ void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega,
}

double vel_linear = std::hypot(vx, vy);
if (vel_linear > max_vel_linear)
if (vel_linear > max_vel_trans)
{
double max_vel_linear_ratio = max_vel_linear / vel_linear;
vx *= max_vel_linear_ratio;
vy *= max_vel_linear_ratio;
double max_vel_trans_ratio = max_vel_trans / vel_linear;
vx *= max_vel_trans_ratio;
vy *= max_vel_trans_ratio;
}
}

Expand Down