From 81f60e543950da82de071bc64fc681ccd0988d38 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20R=C3=B6smann?= Date: Wed, 27 May 2020 10:12:01 +0200 Subject: [PATCH] Dynamic obstacles: the inequality constraints now uses the actual time parameter rather than the time from the previous optimization --- .../optimal_control/stage_inequality_se2.h | 6 +++ .../optimal_control/stage_inequality_se2.cpp | 39 +++++++++++++------ ...mpc_local_planner_params_minimum_time.yaml | 4 +- 3 files changed, 36 insertions(+), 13 deletions(-) diff --git a/mpc_local_planner/include/mpc_local_planner/optimal_control/stage_inequality_se2.h b/mpc_local_planner/include/mpc_local_planner/optimal_control/stage_inequality_se2.h index 6300961..e37744e 100644 --- a/mpc_local_planner/include/mpc_local_planner/optimal_control/stage_inequality_se2.h +++ b/mpc_local_planner/include/mpc_local_planner/optimal_control/stage_inequality_se2.h @@ -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& x_k, Eigen::Ref cost) const override; // implements interface method + void computeNonIntegralStateDtTerm(int k, const Eigen::Ref& x_k, double dt_k, + Eigen::Ref cost) const override; + // implements interface method void computeNonIntegralControlDeviationTerm(int k, const Eigen::Ref& u_k, const Eigen::Ref& u_prev, double dt, Eigen::Ref cost) const override; @@ -102,6 +107,7 @@ class StageInequalitySE2 : public corbo::StageInequalityConstraint protected: const teb_local_planner::ObstContainer* _obstacles = nullptr; std::vector _relevant_obstacles; // TODO: we can also store raw pointers as _via_points is locked by mutex + std::vector _relevant_dyn_obstacles; teb_local_planner::RobotFootprintModelPtr _robot_model; diff --git a/mpc_local_planner/src/optimal_control/stage_inequality_se2.cpp b/mpc_local_planner/src/optimal_control/stage_inequality_se2.cpp index 4d6de49..7a17e2f 100644 --- a/mpc_local_planner/src/optimal_control/stage_inequality_se2.cpp +++ b/mpc_local_planner/src/optimal_control/stage_inequality_se2.cpp @@ -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& /*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(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 @@ -156,15 +166,22 @@ void StageInequalitySE2::computeNonIntegralStateTerm(int k, const Eigen::RefisDynamic()) - { - 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); - } + cost[i] = _min_obstacle_dist - _robot_model->calculateDistance(pose, _relevant_obstacles[k][i].get()); + } +} + +void StageInequalitySE2::computeNonIntegralStateDtTerm(int k, const Eigen::Ref& x_k, double dt_k, + Eigen::Ref 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); } } diff --git a/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_minimum_time.yaml b/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_minimum_time.yaml index c73eb46..0ff945e 100644 --- a/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_minimum_time.yaml +++ b/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_minimum_time.yaml @@ -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