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

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

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<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
@@ -156,15 +166,22 @@ void StageInequalitySE2::computeNonIntegralStateTerm(int k, const Eigen::Ref<con
     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());
-        {
+    }
-            cost[i] = _min_obstacle_dist - _robot_model->calculateDistance(pose, _relevant_obstacles[k][i].get());
+}
-        }
+
-        else
+void StageInequalitySE2::computeNonIntegralStateDtTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, double dt_k,
-        {
+                                                       Eigen::Ref<Eigen::VectorXd> cost) const
-            cost[i] =
+{
-                _min_obstacle_dist - _robot_model->estimateSpatioTemporalDistance(pose, _relevant_obstacles[k][i].get(), (double)k * _current_dt);
+    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);
     }
 }
 

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