12345qiupeng
/
mpc_local_planner
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Dynamic obstacles: the inequality constraints now uses the actual time parameter rather than the time from the previous optimization

master
Christoph Rösmann 2020-05-27 10:12:01 +02:00
parent 203220501a
commit 81f60e5439
3 changed files with 36 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
mpc_local_planner/include/mpc_local_planner/optimal_control/stage_inequality_se2.h
View File

@ -64,6 +64,8 @@ class StageInequalitySE2 : public corbo::StageInequalityConstraint
// implements interface method
int getNonIntegralStateTermDimension(int k) const override;
// implements interface method
int getNonIntegralStateDtTermDimension(int k) const override;
// implements interface method
int getNonIntegralControlDeviationTermDimension(int k) const override;
// implements interface method
@ -75,6 +77,9 @@ class StageInequalitySE2 : public corbo::StageInequalityConstraint
// implements interface method
void computeNonIntegralStateTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, Eigen::Ref<Eigen::VectorXd> cost) const override;
// implements interface method
void computeNonIntegralStateDtTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, double dt_k,
Eigen::Ref<Eigen::VectorXd> cost) const override;
// implements interface method
void computeNonIntegralControlDeviationTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& u_k, const Eigen::Ref<const Eigen::VectorXd>& u_prev,
double dt, Eigen::Ref<Eigen::VectorXd> cost) const override;
@ -102,6 +107,7 @@ class StageInequalitySE2 : public corbo::StageInequalityConstraint
protected:
const teb_local_planner::ObstContainer* _obstacles = nullptr;
std::vector<teb_local_planner::ObstContainer> _relevant_obstacles; // TODO: we can also store raw pointers as _via_points is locked by mutex
std::vector<teb_local_planner::ObstContainer> _relevant_dyn_obstacles;
teb_local_planner::RobotFootprintModelPtr _robot_model;

33
mpc_local_planner/src/optimal_control/stage_inequality_se2.cpp
View File

@ -39,6 +39,12 @@ int StageInequalitySE2::getNonIntegralStateTermDimension(int k) const
return (int)_relevant_obstacles[k].size();
}
int StageInequalitySE2::getNonIntegralStateDtTermDimension(int k) const
{
assert(k < _relevant_dyn_obstacles.size());
return (int)_relevant_dyn_obstacles[k].size();
}
bool StageInequalitySE2::update(int n, double /*t*/, corbo::ReferenceTrajectoryInterface& /*xref*/, corbo::ReferenceTrajectoryInterface& /*uref*/,
corbo::ReferenceTrajectoryInterface* /*sref*/, bool /* single_dt*/, const Eigen::VectorXd& x0,
corbo::StagePreprocessor::Ptr /*stage_preprocessor*/, const std::vector<double>& /*dts*/,
@ -54,9 +60,10 @@ bool StageInequalitySE2::update(int n, double /*t*/, corbo::ReferenceTrajectoryI
// Alternatively, other grids could be used in combination with method getStateAndControlTimeSeries()
const FullDiscretizationGridBaseSE2* fd_grid = static_cast<const FullDiscretizationGridBaseSE2*>(grid);
bool new_dimensions = (n != _relevant_obstacles.size());
bool new_dimensions = (n != _relevant_obstacles.size()) || (n != _relevant_dyn_obstacles.size());
_relevant_obstacles.resize(n);
_relevant_dyn_obstacles.resize(n);
teb_local_planner::PoseSE2 pose;
@ -78,6 +85,8 @@ bool StageInequalitySE2::update(int n, double /*t*/, corbo::ReferenceTrajectoryI
int num_prev_obst = (int)_relevant_obstacles[k].size();
_relevant_obstacles[k].clear();
int num_prev_dyn_obst = (int)_relevant_dyn_obstacles[k].size();
_relevant_dyn_obstacles[k].clear();
// iterate obstacles
for (const ObstaclePtr& obst : *_obstacles)
@ -88,7 +97,7 @@ bool StageInequalitySE2::update(int n, double /*t*/, corbo::ReferenceTrajectoryI
// we consider all dynamic obstacles by now
// TODO(roesmann): we might remove obstacles that "go away" from the trajectory
// or more generally that any intersection in the future is unlikely
_relevant_obstacles[k].push_back(obst);
_relevant_dyn_obstacles[k].push_back(obst);
continue;
}
@ -129,6 +138,7 @@ bool StageInequalitySE2::update(int n, double /*t*/, corbo::ReferenceTrajectoryI
// check if dimensions changed
new_dimensions = new_dimensions || (_relevant_obstacles[k].size() != num_prev_obst);
new_dimensions = new_dimensions || (_relevant_dyn_obstacles[k].size() != num_prev_dyn_obst);
}
// update current dt
@ -155,16 +165,23 @@ void StageInequalitySE2::computeNonIntegralStateTerm(int k, const Eigen::Ref<con
// TODO(roesmann): Overload robot fooprint model functions in teb_local_planner to avoid this copy:
teb_local_planner::PoseSE2 pose(x_k[0], x_k[1], x_k[2]);
for (int i = 0; i < (int)_relevant_obstacles[k].size(); ++i)
{
if (!_enable_dynamic_obstacles || !_relevant_obstacles[k][i]->isDynamic())
{
cost[i] = _min_obstacle_dist - _robot_model->calculateDistance(pose, _relevant_obstacles[k][i].get());
}
else
{
cost[i] =
_min_obstacle_dist - _robot_model->estimateSpatioTemporalDistance(pose, _relevant_obstacles[k][i].get(), (double)k * _current_dt);
}
void StageInequalitySE2::computeNonIntegralStateDtTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, double dt_k,
Eigen::Ref<Eigen::VectorXd> cost) const
{
assert(_obstacles);
assert(k < _relevant_dyn_obstacles.size());
assert(cost.size() == _relevant_dyn_obstacles[k].size());
// TODO(roesmann): Overload robot fooprint model functions in teb_local_planner to avoid this copy:
teb_local_planner::PoseSE2 pose(x_k[0], x_k[1], x_k[2]);
for (int i = 0; i < (int)_relevant_dyn_obstacles[k].size(); ++i)
{
cost[i] = _min_obstacle_dist - _robot_model->estimateSpatioTemporalDistance(pose, _relevant_dyn_obstacles[k][i].get(), (double)k * dt_k);
}
}

4
mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_minimum_time.yaml
View File

@ -70,14 +70,14 @@ MpcLocalPlannerROS:
## Controller options
controller:
outer_ocp_iterations: 1
outer_ocp_iterations: 5
xy_goal_tolerance: 0.2
yaw_goal_tolerance: 0.1
global_plan_overwrite_orientation: true
global_plan_prune_distance: 1.0
allow_init_with_backward_motion: True
max_global_plan_lookahead_dist: 1.5
global_plan_viapoint_sep: 0.5
global_plan_viapoint_sep: 5.5
force_reinit_new_goal_dist: 1.0
force_reinit_new_goal_angular: 1.57
force_reinit_num_steps: 0