rst-tu-dortmund/mpc_local_planner

The mpc_local_planner package implements a plugin to the base_local_planner of the 2D navigation stack. It provides a generic and versatile model predictive control implementation with minimum-time and quadratic-form receding-horizon configurations.

mpc_local_planner ROS Package

The mpc_local_planner package implements a plugin to the base_local_planner of the 2D navigation stack. It provides a generic and versatile model predictive control implementation with minimum-time and quadratic-form receding-horizon configurations.

For custom build instructions (e.g. compilation with other third-party solvers) see this wiki.

Refer to http://wiki.ros.org/mpc_local_planner for more general information and tutorials.

Build status:

• ROS Melodic (melodic-devel):

Authors

• Christoph Rösmann [email protected]

Citing the Software

Since a lot of time and effort has gone into the development, please cite at least one of the following publications if you are using the software for published work.

Main Paper and Approach

• C. Rösmann, A. Makarow, and T. Bertram: Online Motion Planning based on Nonlinear Model Predictive Control with Non-Euclidean Rotation Groups, arXiv:2006.03534 '[cs.RO]', June 2020.

Standard MPC and Hypergraph

• C. Rösmann, M. Krämer, A. Makarow, F. Hoffmann and T. Bertram: Exploiting Sparse Structures in Nonlinear Model Predictive Control with Hypergraphs, IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM), New Zealand, July 2018.

Time-Optimal MPC and Hypergraph

• C. Rösmann: Time-optimal nonlinear model predictive control, Direct transcription methods with variable discretization and structural sparsity exploitation. Dissertation, Technische Universität Dortmund, Oct. 2019.
• C. Rösmann, F. Hoffmann und T. Bertram: Timed-Elastic-Bands for Time-Optimal Point-to-Point Nonlinear Model Predictive Control, European Control Conference (ECC), Austria, July 2015.

License

Copyright (c) 2020, Christoph Rösmann, All rights reserved.

This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with this program. If not, see https://www.gnu.org/licenses/.

The package depends on third-party packages:

• control_box_rst, GPLv3, https://github.com/rst-tu-dortmund/control_box_rst Please check also the dependencies of control_box_rst (not all modules are included at the moment)
• Ipopt, EPL 1.0, https://github.com/coin-or/Ipopt

It depends on other ROS packages, which are listed in the package.xml. They are licensed under BSD resp. Apache 2.0.

Requirements

Install dependencies (listed in the package.xml and CMakeLists.txt file) using rosdep:

rosdep install mpc_local_planner

mpc_local_planner's Issues

cost function

Hello, I am new to this and I got a little confused when I try to find the cost funtion of the MPC process. I want to alter the cost function like adding some term to it. And I wonder if there an explicit way of setting the cost function. Thank you so much!

mpc_local_planner can achieved a turn around in place？

I cant achieve a turn around in place for the two wheel differential model，what can I do? how can I do ? Thank you

about supporting for ros kinetic

hello!
can this mpc local planner support ros-kinetic?
I can't compile the code successfully in ros-kinetic.

catkin_make error

/home/dell/ws/src/mpc_local_planner/mpc_local_planner/src/test_mpc_optim_node.cpp:184:111: error: ‘boost::placeholders’ has not been declared
marker_server->setCallback(i_marker.name, boost::bind(&TestMpcOptimNode::CB_obstacle_marker, this, boost::placeholders::_1));
^~~~~~~~~~~~
mpc_local_planner/mpc_local_planner/CMakeFiles/test_mpc_optim_node.dir/build.make:62: recipe for target 'mpc_local_planner/mpc_local_planner/CMakeFiles/test_mpc_optim_node.dir/src/test_mpc_optim_node.cpp.o' failed
make[2]: *** [mpc_local_planner/mpc_local_planner/CMakeFiles/test_mpc_optim_node.dir/src/test_mpc_optim_node.cpp.o] Error 1
CMakeFiles/Makefile2:9542: recipe for target 'mpc_local_planner/mpc_local_planner/CMakeFiles/test_mpc_optim_node.dir/all' failed
make[1]: *** [mpc_local_planner/mpc_local_planner/CMakeFiles/test_mpc_optim_node.dir/all] Error 2
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j8 -l8" failed

ROS Noetic crash on receiving nav goal

I am using a polygon model and whenever I send a navGoal, it crashes. I am using the mpc_local_planner_params_quadratic_form.yaml file for my robot and when I keep it as a point model, it doesn't crash. It only crashes once I switch to a polygon model. Any idea what I am doing wrong?

Removing dependency on teb_local_planner

The current implementation shares a lot of objects with the teb_local_planner library, e.g. PoseSE2, obstacles, robot footprints etc.
I plan to move those objects to a separate planning_utilities library, however, since this also affects the teb_local_planner code, I am waiting for future distributions like noetic or ros2 to apply this change. If somebody has any reasons for not doing so, please let me know ;-)

Catkin build error due to missing arg

This pkg fails to compile due to the missing min_obstacle_dist argument required for creating the LineRobotFootprint instance when using the Eigen version.

Created a PR fix for the same.

how to install mpc local planner for four wheel steering robot ?

I recently referenced four wheel steering robot simulation:
https://www.youtube.com/watch?v=4_bftUbQTaM&t=91s

I want to try to apply mpc local planner to a 4-wheel steering robot and observe the effect of path planning, but,the examples are all for differential or car-like robots , not 4-wheel steering robot.

What should I do to make it work with 4 wheel steer vehicle configuration?？Does anyone know how to do this？

Noetic Catkin build error Ubunto 20.04

Hi Im having the following error:
In file included from /usr/include/boost/smart_ptr/make_shared.hpp:14,
from /usr/include/boost/make_shared.hpp:14,
from /opt/ros/noetic/include/ros/forwards.h:38,
from /opt/ros/noetic/include/ros/common.h:37,
from /opt/ros/noetic/include/ros/ros.h:43,
from /home/usr/data/ExplOrbSLAM_working/src/mpc_local_planner/mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h:26,
from /home/usr/data/ExplOrbSLAM_working/src/mpc_local_planner/mpc_local_planner/src/mpc_local_planner_ros.cpp:23:
/usr/include/boost/smart_ptr/make_shared_object.hpp: In instantiation of ‘typename boost::detail::sp_if_not_array::type boost::make_shared(Args&& ...) [with T = teb_local_planner::LineRobotFootprint; Args = {Eigen::Map<const Eigen::Matrix<double, 2, 1, 0, 2, 1>, 0, Eigen::Stride<0, 0> >, Eigen::Map<const Eigen::Matrix<double, 2, 1, 0, 2, 1>, 0, Eigen::Stride<0, 0> >}; typename boost::detail::sp_if_not_array::type = boost::shared_ptr<teb_local_planner::LineRobotFootprint>]’:
/home/usr/data/ExplOrbSLAM_working/src/mpc_local_planner/mpc_local_planner/src/mpc_local_planner_ros.cpp:924:124: required from here
/usr/include/boost/smart_ptr/make_shared_object.hpp:256:5: error: no matching function for call to ‘teb_local_planner::LineRobotFootprint::LineRobotFootprint(Eigen::Map<const Eigen::Matrix<double, 2, 1>, 0, Eigen::Stride<0, 0> >, Eigen::Map<const Eigen::Matrix<double, 2, 1>, 0, Eigen::Stride<0, 0> >)’
256 | ::new( pv ) T( boost::detail::sp_forward( args )... );
| ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In file included from /home/usr/data/ExplOrbSLAM_working/src/mpc_local_planner/mpc_local_planner/include/mpc_local_planner/optimal_control/stage_inequality_se2.h:29,
from /home/usr/data/ExplOrbSLAM_working/src/mpc_local_planner/mpc_local_planner/include/mpc_local_planner/controller.h:29,
from /home/usr/data/ExplOrbSLAM_working/src/mpc_local_planner/mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h:36,
from /home/usr/data/ExplOrbSLAM_working/src/mpc_local_planner/mpc_local_planner/src/mpc_local_planner_ros.cpp:23:
/home/usr/data/ExplOrbSLAM_working/src/teb_local_planner/include/teb_local_planner/robot_footprint_model.h:458:3: note: candidate: ‘teb_local_planner::LineRobotFootprint::LineRobotFootprint(const Vector2d&, const Vector2d&, double)’
458 | LineRobotFootprint(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, const double min_obstacle_dist) : min_obstacle_dist_(min_obstacle_dist)
| ^~~~~~~~~~~~~~~~~~
/home/usr/data/ExplOrbSLAM_working/src/teb_local_planner/include/teb_local_planner/robot_footprint_model.h:458:3: note: candidate expects 3 arguments, 2 provided
/home/usr/data/ExplOrbSLAM_working/src/teb_local_planner/include/teb_local_planner/robot_footprint_model.h:448:3: note: candidate: ‘teb_local_planner::LineRobotFootprint::LineRobotFootprint(const Point&, const Point&)’
448 | LineRobotFootprint(const geometry_msgs::Point& line_start, const geometry_msgs::Point& line_end)
| ^~~~~~~~~~~~~~~~~~
/home/usr/data/ExplOrbSLAM_working/src/teb_local_planner/include/teb_local_planner/robot_footprint_model.h:448:50: note: no known conversion for argument 1 from ‘Eigen::Map<const Eigen::Matrix<double, 2, 1>, 0, Eigen::Stride<0, 0> >’ to ‘const Point&’ {aka ‘const geometry_msgs::Point_<std::allocator >&’}
448 | LineRobotFootprint(const geometry_msgs::Point& line_start, const geometry_msgs::Point& line_end)
| ~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~
/home/usr/data/ExplOrbSLAM_working/src/teb_local_planner/include/teb_local_planner/robot_footprint_model.h:439:7: note: candidate: ‘teb_local_planner::LineRobotFootprint::LineRobotFootprint(const teb_local_planner::LineRobotFootprint&)’
439 | class LineRobotFootprint : public BaseRobotFootprintModel
| ^~~~~~~~~~~~~~~~~~
/home/usr/data/ExplOrbSLAM_working/src/teb_local_planner/include/teb_local_planner/robot_footprint_model.h:439:7: note: candidate expects 1 argument, 2 provided
make[2]: *** [CMakeFiles/mpc_local_planner.dir/build.make:76: CMakeFiles/mpc_local_planner.dir/src/mpc_local_planner_ros.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:2896: CMakeFiles/mpc_local_planner.dir/all] Error 2
make: *** [Makefile:141: all] Error 2

stuck at a narrow doorway

Hi developers,

Thanks for your contribution. I am running the carlike_minimum_time example provided by the repo, setting the 2d goal at [2.87, 7.78]. However, the robot car will be stuck at the narrow doorway as follows. Hereby, I am wondering is it the limitation of the MPC planner? Or, some hyperparameters are not be set properly (I have tried some different hypermeters at mpc_local_planner_params.yaml).

Looking forward to your reply : )

ubuntu20.04-catkin_make error

1 error: ‘using element_type = class corbo::SolverIpopt’ {aka ‘class corbo::SolverIpopt’} has no member named ‘setIterations’
2 error: ‘using element_type = class corbo::SolverIpopt’ {aka ‘class corbo::SolverIpopt’} has no member named ‘setIpoptOptionString’
3 error: no matching function for call to ‘teb_local_planner::LineRobotFootprint::LineRobotFootprint(Eigen::Map<const Eigen::Matrix<double, 2, 1>, 0, Eigen::Stride<0, 0> >, Eigen::Map<const Eigen::Matrix<double, 2, 1>, 0, Eigen::Stride<0, 0> >)’

my command show as follow, how do i solve it?

/home/lin/ros_code/nav_ws/src/mpc_local_planner/mpc_local_planner/src/controller.cpp: In member function ‘corbo::NlpSolverInterface::Ptr mpc_local_planner::Controller::configureSolver(const ros::NodeHandle&)’:
/home/lin/ros_code/nav_ws/src/mpc_local_planner/mpc_local_planner/src/controller.cpp:393:17: error: ‘using element_type = class corbo::SolverIpopt’ {aka ‘class corbo::SolverIpopt’} has no member named ‘setIterations’
393 | solver->setIterations(iterations);
| ^~~~~~~~~~~~~
/home/lin/ros_code/nav_ws/src/mpc_local_planner/mpc_local_planner/src/controller.cpp:397:17: error: ‘using element_type = class corbo::SolverIpopt’ {aka ‘class corbo::SolverIpopt’} has no member named ‘setMaxCpuTime’
397 | solver->setMaxCpuTime(max_cpu_time);
| ^~~~~~~~~~~~~
/home/lin/ros_code/nav_ws/src/mpc_local_planner/mpc_local_planner/src/controller.cpp:404:26: error: ‘using element_type = class corbo::SolverIpopt’ {aka ‘class corbo::SolverIpopt’} has no member named ‘setIpoptOptionNumeric’
404 | if (!solver->setIpoptOptionNumeric(item.first, item.second)) ROS_WARN_STREAM("Ipopt option " << item.first << " could not be set.");
| ^~~~~~~~~~~~~~~~~~~~~
/home/lin/ros_code/nav_ws/src/mpc_local_planner/mpc_local_planner/src/controller.cpp:411:26: error: ‘using element_type = class corbo::SolverIpopt’ {aka ‘class corbo::SolverIpopt’} has no member named ‘setIpoptOptionString’
411 | if (!solver->setIpoptOptionString(item.first, item.second)) ROS_WARN_STREAM("Ipopt option " << item.first << " could not be set.");
| ^~~~~~~~~~~~~~~~~~~~
/home/lin/ros_code/nav_ws/src/mpc_local_planner/mpc_local_planner/src/controller.cpp:418:26: error: ‘using element_type = class corbo::SolverIpopt’ {aka ‘class corbo::SolverIpopt’} has no member named ‘setIpoptOptionInt’
418 | if (!solver->setIpoptOptionInt(item.first, item.second)) ROS_WARN_STREAM("Ipopt option " << item.first << " could not be set.");
| ^~~~~~~~~~~~~~~~~
In file included from /usr/include/boost/smart_ptr/make_shared.hpp:14,
from /usr/include/boost/make_shared.hpp:14,
from /opt/ros/noetic/include/ros/forwards.h:38,
from /opt/ros/noetic/include/ros/common.h:37,
from /opt/ros/noetic/include/ros/ros.h:43,
from /home/lin/ros_code/nav_ws/src/mpc_local_planner/mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h:26,
from /home/lin/ros_code/nav_ws/src/mpc_local_planner/mpc_local_planner/src/mpc_local_planner_ros.cpp:23:
/usr/include/boost/smart_ptr/make_shared_object.hpp: In instantiation of ‘typename boost::detail::sp_if_not_array::type boost::make_shared(Args&& ...) [with T = teb_local_planner::LineRobotFootprint; Args = {Eigen::Map<const Eigen::Matrix<double, 2, 1, 0, 2, 1>, 0, Eigen::Stride<0, 0> >, Eigen::Map<const Eigen::Matrix<double, 2, 1, 0, 2, 1>, 0, Eigen::Stride<0, 0> >}; typename boost::detail::sp_if_not_array::type = boost::shared_ptr<teb_local_planner::LineRobotFootprint>]’:
/home/lin/ros_code/nav_ws/src/mpc_local_planner/mpc_local_planner/src/mpc_local_planner_ros.cpp:925:124: required from here
/usr/include/boost/smart_ptr/make_shared_object.hpp:256:5: error: no matching function for call to ‘teb_local_planner::LineRobotFootprint::LineRobotFootprint(Eigen::Map<const Eigen::Matrix<double, 2, 1>, 0, Eigen::Stride<0, 0> >, Eigen::Map<const Eigen::Matrix<double, 2, 1>, 0, Eigen::Stride<0, 0> >)’
256 | ::new( pv ) T( boost::detail::sp_forward( args )... );
| ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

What's the difference between mpc_local_planner and teb_local_planner and dwa_local_planner?

Why all three of them can be members of mpc, as you wrote it in the paper.
DWA
For example the dynamic window approach (DWA) [11] can be interpreted as a special receding horizon MPC scheme [12].
TEB
Another approach is the Timed-Elastic-Band (TEB) approach, which was originally derived as a scalarized multi-objective least squares optimization [13]. It can also be derived from a time-optimal control problem as shown in the right of Fig. 1.

[1] C. Rösmann, A. Makarow, and T. Bertram: Online Motion Planning based on Nonlinear Model Predictive Control with Non-Euclidean Rotation Groups, arXiv:2006.03534 '[cs.RO]', June 2020.

Support for obstacle inflation

Currently, the mpc_local_planner considers obstacles only by hard-constraints.
This allows the planner to pass obstacles very close by without even trying to increase the distance. The DWA planner tackles this problem by taking an inflation layer from costmap_2d into account. The teb_local_planner also provides an inflation concept based on penalty cost functions.

It is also possible to add an inflation concept to the mpc_local_planner, however, an efficient implementation that shares distance computations for both penalty costs and hard-constraints is a bit difficult (but not impossible ;-)) with the current API. So before I start working on this, I would like to wait for some test reports and user experiences.

melodic catkin_make error

hi, I have successfully installed ipopt and control_box_rst, but when i run catkin_make , i got the following error:

[ 29%] Building CXX object mpc_local_planner/mpc_local_planner/CMakeFiles/mpc_local_planner_optimal_control.dir/src/optimal_control/stage_inequality_se2.cpp.o
In file included from /home/glh/ROS_WS/mpc_ws/src/mpc_local_planner/mpc_local_planner/src/optimal_control/stage_inequality_se2.cpp:24:0:
/home/glh/ROS_WS/mpc_ws/src/mpc_local_planner/mpc_local_planner/include/mpc_local_planner/optimal_control/stage_inequality_se2.h:67:9: error: ‘int mpc_local_planner::StageInequalitySE2::getNonIntegralStateDtTermDimension(int) const’ marked ‘override’, but does not override
     int getNonIntegralStateDtTermDimension(int k) const override;
         ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In file included from /home/glh/ROS_WS/mpc_ws/src/mpc_local_planner/mpc_local_planner/src/optimal_control/stage_inequality_se2.cpp:24:0:
/home/glh/ROS_WS/mpc_ws/src/mpc_local_planner/mpc_local_planner/include/mpc_local_planner/optimal_control/stage_inequality_se2.h:80:10: error: ‘void mpc_local_planner::StageInequalitySE2::computeNonIntegralStateDtTerm(int, const Eigen::Ref<const Eigen::Matrix<double, -1, 1> >&, double, Eigen::Ref<Eigen::Matrix<double, -1, 1> >) const’ marked ‘override’, but does not override
     void computeNonIntegralStateDtTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, double dt_k,
          ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mpc_local_planner/mpc_local_planner/CMakeFiles/mpc_local_planner_optimal_control.dir/build.make:134: recipe for target 'mpc_local_planner/mpc_local_planner/CMakeFiles/mpc_local_planner_optimal_control.dir/src/optimal_control/stage_inequality_se2.cpp.o' failed
make[2]: *** [mpc_local_planner/mpc_local_planner/CMakeFiles/mpc_local_planner_optimal_control.dir/src/optimal_control/stage_inequality_se2.cpp.o] Error 1
CMakeFiles/Makefile2:625: recipe for target 'mpc_local_planner/mpc_local_planner/CMakeFiles/mpc_local_planner_optimal_control.dir/all' failed
make[1]: *** [mpc_local_planner/mpc_local_planner/CMakeFiles/mpc_local_planner_optimal_control.dir/all] Error 2
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j8 -l8" failed

move_base error

when i put
roslaunch mpc_local_planner_examples carlike_minimum_time.launch
everything is fine, no error and warning.
but when i set a goal with 2D Nav Goal. error happens
here is the error msg:

[move_base-3] process has died [pid 240428, exit code -11, cmd /opt/ros/noetic/lib/move_base/move_base __name:=move_base __log:=/home/loren/.ros/log/b04678b0-fc67-11ed-bd54-d1dcc34a72bd/move_base-3.log].
log file: /home/loren/.ros/log/b04678b0-fc67-11ed-bd54-d1dcc34a72bd/move_base-3*.log

Dose anyone can help me out?
I use the noetic-version
Thanks

Error install dependencies

When I try to install dependencies with "rosdep install mpc_local_planner" I have this error :
ERROR: Rosdep cannot find all required resources to answer your query
Missing resource mpc_local_planner
ROS path [0]=/opt/ros/melodic/share/ros
ROS path [1]=/home/bperrote/catkin_ws/src
ROS path [2]=/opt/ros/melodic/share

ros-foxy distribution.

(this is a question post).
The question is if the ROS-foxy distribution is available yet or not?

corbo core error file missing

CMake Error at mpc_local_planner/mpc_local_planner/CMakeLists.txt:206 (add_dependencies):
The dependency target "corbo_core" of target "mpc_local_planner_utils" does
not exist.

CMake Error at mpc_local_planner/mpc_local_planner/CMakeLists.txt:206 (add_dependencies):
The dependency target "corbo_systems" of target "mpc_local_planner_utils"
does not exist.

Mpc Planner with Navigation Stack and car urdf model - real dimensions

Hi George Kouros, I am doing a research with local planners in ROS1. I have used the TEB local planner for that in a urdf car model (The prius model) in Gazebo. This planner is working quite good, however the DWA and Eband and others just work for holonomic robots...Whatever my doubt is how to use properly the mpc_local_planner in my simulation. I have already configured the cfg parameters with similar values from TEB (obstacle layer, inflation layer, obstacle avoidance, the robot footprint and dimensions). However MPC has different parameters to be set in "mpc_local_planner_params.yaml", (such as Planning Grid, Planning Options, Controller Options, Solver Settings and Model), that I do not know to set it and their exact meaning..
I have tried to change them in order to improve the car behaviour in my simulation. However the car almost never achieves the goal, always start driving backward (Even when I have disabled to start with backward move), and hardly find and follow a feasible path to avoid an obstacle (it recalculates every time a new path and therefore never finishes a maneuver to escape the obstacle).
Besides that my research involves make a benchmark of local planners. I have read your article to understand better the algorithm. I wish to know how can I extract from navigation stack these comparison metrics (such as total path length, planning Time, CPU consumption, Smoothness of the paths, Number of Attempts and Success of planning, etc...).
I am kind of worried, beacause I do not find suitable local planners to try and compare in my simulation master research. I know that MoveIt for planning robot's arms moves allow easily switch SBPL and OMPL planners and has already a lot of benchmark features to compare the algorithms. As this is not standard given by navigation stack, I imagine I need to write the scripts to do that. Could you please help me with some Examples of your algorithm working with a model in Gazebo? Then I could set similarly. Also if possible to tell me which topics could I subscribe to get those benchmark features you present in the article, or how to code it? I also appreciate any suggestion of framework linked to ROS that I could use to assess path planning algorithms and compare them ( in a not so complex way, as I am a mechanical engineer doing master in robotics, not a computer scientist expert...)
In addition I also installed the rsband planner in my workspace, installed dependencies and used it in naviagation stack...it is displayed in Rviz. However I am sure I have not correctley set the parameters, once this planner also does not avoid collision and behave not so good for this goal ( I mean I do not know to properly set it and just copied some parameters with same value of Eband). Do you have a gazebo example of it to launch?
Thanks very much in advance!
And congratulations for the paper and the TEB and MPC planners!

Distinctive topologies for mpc_local_planner

Hi @croesmann.
I am using this mpc_local_planner for a project. I know teb_local_planner has a feature called distinctive topologies (parallel planning). Does mpc_local_planner have this feature or do you have any plans to add it to this mpc local planner?
What are possible challenges and roadblocks?

ROS melodic installation using source installation of control_box_rst

When I try to build the mpc local planner with control_box_rst source installed. I get the following error

/usr/bin/ld: cannot find -llibyamlcpp
/usr/bin/ld: cannot find -llibprotobuf
/usr/bin/ld: cannot find -llibgrpc
collect2: error: ld returned 1 exit status
make[2]: *** [/home/chandra/mpc_ws/devel/.private/mpc_local_planner/lib/libmpc_local_planner_utils.so] Error 1
make[1]: *** [CMakeFiles/mpc_local_planner_utils.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
/usr/bin/ld: cannot find -llibyamlcpp
/usr/bin/ld: cannot find -llibprotobuf
/usr/bin/ld: cannot find -llibgrpc
collect2: error: ld returned 1 exit status
make[2]: *** [/home/chandra/mpc_ws/devel/.private/mpc_local_planner/lib/libmpc_local_planner_optimal_control.so] Error 1
make[1]: *** [CMakeFiles/mpc_local_planner_optimal_control.dir/all] Error 2
make: *** [all] Error 2

Could you please help me solve this issue.

Real robot go recoil when entering the curve

I'm testing mpc for my robot, it run normal in simulation. But in real, robot go recoil when entering the curve.
Like that video

Integration with homotopy class planning

Similar to the teb_local_planner, the mpc_local_planner is subject to local optimization and might get stuck in local minima occurring from obstacles passing by. This can be partially avoided by deactivating grid/warm_starting or by setting controller/force_reinit_num_steps to a positive integer. However, an integration with the homotopy class planner of the teb_local_planner package is straightforward, so I would like to add that to my roadmap.
I would like to move that part of code according to #2 to avoid redundant code.

catkin_make failed due to Permission denied

Hi

I follow the steps for installing the mpc_local_planner. However, when i tried with catkin_make, it failed with Permission denied on the following items:

1. mpc_footprint.cfg
2. mpc_controller.cfg
3. mpc_collision.cfg

Tried with re- git clone and re- catkin_make, but the results still the same.

Best,
Samuel

Unify the 3 dynamic reconfigure servers into one (retaining tabs)

Hi, this issue is both a question and a proposal.

question:
what are the benefits of splitting the dynamic reconfigurable parameters into 3 servers? The drawback I found is that we cannot treat MPC as any other local planner when changing generic parameters as the goal tolerances.

proposal:
replicate TEB architecture with a single server but multiple tabs, so parameters are neatly distributed on RQT reconfigure but still accessible under a predictably named server (we will PR the change if deemed opportune).

Dynamic car model

Hi @croesmann,

Currently in the repo, as well as in your article, there is only kinematic car model considered. I wonder if you've tested dynamic model? Even if not, maybe you still have some "predictions" about accuracy vs. efficiency trade-off

Regards,

Mpc local planner vs Teb local planner vs Dwa local planner

Anyone can tell me the difference between Mpc local planner, Teb local planner and Dwa local planner ???
Can you explain for me in detailed about Advantages and Disadvantages
Thank you so much !!!

Corbo dependencies making caktin_make fail

Hello,

I have been trying to build my project for the last two days without any success. I keep getting corbo dependencies errors, even though I have installed the control_box_rst package and I doubled checked that it is on the include directory of ROS melodic. Can you give me a hand on this one?

I am using ROS Melodic with C++ and I have installed both packages, mpc_local_planner and control_box_rst, using the installation instructions on the official github.

This is the error I am getting every time I try to build the project. It is either corbo-controllers or corbo-core giving me a headache. The only way to avoid this error is to not include any of the mpc_local_planner headers.

This is proof that I have the control_box_rst installed and I also installed the mpc_local_planner package:

And this is part of my CMakeLists.txt file:

Robustification w.r.t. state constraints

It is well known in MPC that state constraints heavily affect robustness properties.

Consider the following example: the robot is not allowed to drive backwards, but sensor noise provides values around zero (also small negative values). For these negative cases, the underlying optimal control problem is infeasible.
The same might happen when the robot moves at the maximum velocity and sensor noise leads to an exceed or an external force further accelerates the robot. Another reason could be a potential collision in the computed planning scene but not in the real world.

A possible way to tackle this issue is to convert state hard-constraints to soft-constraints, however, this is not always desired (e.g. for collision avoidance). One could also convert hard-constraints to soft-constraints as a recovery strategy. Another way is to overwrite the noisy sensor data by the actual limits (but I don't like such "workarounds" in general).
Or one could rely just on the robot to brake in case of infeasibility and "hope" to recover automatically (this is the current implementation).

I would like to collect some user experiences and test results before I continue working on this topic.

How to enable mpc path tracking only

Hi

I would like to try out the mpc on path tracking only, as i have my own path generating on local plan. What parameters shall i set in order to achieve this?

Best,
Samuel

Crash when goal is received

Hi,

When I try to run planner with move_base, it crashes after receiving goal destination. I get this error message:

[move_base-1] process has died [pid 98384, exit code -11, cmd /opt/ros/noetic/lib/move_base/move_base __name:=move_base __log:=/home/el/.ros/log/0646202c-bc9a-11eb-ac31-7f3e1369d57f/move_base-1.log].
log file: /home/el/.ros/log/0646202c-bc9a-11eb-ac31-7f3e1369d57f/move_base-1.log
all processes on machine have died, roslaunch will exit*
shutting down processing monitor...
... shutting down processing monitor complete
done

I checked log and it's empty.
There is no such crash with TEB planner.

Has anyone encountered such problem? Do you have ideas what is the problem?

Clone failed when I Install Control-Box-RST

fatal: unable to access 'https://boringssl.googlesource.com/boringssl/': Failed to connect to boringssl.googlesource.com port 443
I used a Chinese server and used a proxy，What should I do？

MPC cant tracking path properly and stuck

Hi, @croesmann, I'm working on a project and I'm planning to use MPC cuz several reasons, one of them TEB carlike config causes oscillation. So, I started to use MPC. However MPC couldn't generate proper path or tracking trajectory for the tricycle vehicle even if there was a huge gap to pass though. On the other hand TEB can able to produce path and vehicle follow this path (with oscillation) to goal. At the end, I'm investigating the solution to stuck, produce path and path tracking for three wheeled vehicle.

My mpc config file down below.
mpc_carlike.txt
-- I tried several configurations such as force_inclusion_dist or solver and planner types but none of them can't solved my problem.
-- Costmap params for global map 1.2 both inflation and cost scaling fac. For local cost map: 0.8 cost scaling and 1.2 inflation
Note: Even if I reduce the costmap params same behavior occurs.

-In the first picture mpc giving up the track path and stop.
-In the second and third picture mpc cant turning even there was a proper gap and plan.
-In the last two picture mpc started oscillation and stuck.

What's the difference between mpc_local_planner and mpc trajectory tracking control?

and what's the difference between mpc_planner and mpc_controller?
Hope you can elaborate it, thanks a lot.

Add dynamic parameter server to modify parameters during execution

It would be really helpful to use ROS dynamic reconfigure package to modify several (or all) parameters during execution, as in teb local planner package. This would help the tuning process a lot.
I am about to work on this and submit a PR soon :)

ROS2 navigation2 controller

Hi @croesmann, are there any plans for a ROS2 integration with nav2 stack similar like in teb? There's ros2-master branch, but seems like it doesn't implement nav2_core/controller.hpp interface

Regards,

Build fails in kinetic - missing headers

when i run catkin_make, i got the following error:
In file included from home/glh/navigation/src/mpc_local_planner/mpc_local_planner/src/optimal_control/full_discretization_grid_base_se2.cpp:23:0: /home/glh/navigation/src/mpc_local_planner/mpc_local_planner/include/mpc_local_planner/optimal_control/full_discretization_grid_base_se2.h:26:101: fatal error: corbo-optimal-control/structured_ocp/discretization_grids/discretization_grid_interface.h: No such file or directory
and
In file included from /home/glh/navigation/src/mpc_local_planner/mpc_local_planner/src/utils/publisher.cpp:23:0: /home/glh/navigation/src/mpc_local_planner/mpc_local_planner/include/mpc_local_planner/utils/publisher.h:26:36: fatal error: corbo-core/time_series.h: No such file or directory

ros-kinetic support

I wanted to use for ros-kinetic. I did the source install of control-box-rst package. But when I build the mpc_local_planner package. I get the following error. Could you please help me to solve this issue.

'''
In file included from pathplanner_ws/src/mpc_local_planner/mpc_local_planner/include/mpc_local_planner/utils/publisher.h:26:0,
from pathplanner_ws/src/mpc_local_planner/mpc_local_planner/src/utils/publisher.cpp:23:
/usr/local/include/control_box_rst/corbo-core/time_series.h:31:62: fatal error: corbo-communication/messages/core/time_series.pb.h: No such file or directory
’’’

Issue with release version

Hi

I faced an issue while using the apt-get install ros-melodic-mpc-local-planner, the planner cant seem to generate a path and will be stuck trying to find a path.

I managed to solve this by installing the package from the git hub repo, I was wondering if it is possible to update the apt-get install version of the package.

Thank you.

Dynamic Obstacles on MPC Local Planner

Hi we try to implement a dynamic obstacle publisher for the MPC Local planner based on the object detection that we get from Lidar, so we publish a /ObstacleArrayMsg message type to /move_base/MpcLocalPlannerROS/obstacles topic based on this tutorial incorporating dynamic obstacle tutorial from TEB Local Planner. But we only published the position and pose of the obstacles (polygon shape) without the velocity of the obstacles.

We also have set enable_dynamic_obstacles: True

However we have following problems:

• When we tested it in simulation through gazebo, the obstacle appears in the RViz but when we run the MPC local planner to plan until the goal points, the MPC local path go through the obstacle and not doing any replanning
  

• If we do the test_optim_node and publish the dynamic obstacle message, the MPC local planner wants to replan its path
  

Also my question, do we need to add velocity to the dynamic obstacle to make it works? Thank you

Put MPC in four wheels omnidirecional robot

Good afternoon,
I have a four-wheel omnidirectional robot and I would like to apply MPC, has anyone done something similar? The examples are all for differential robots and the controller is not prepared for an omnidirectional robot.

Does anyone have an idea of how I can easily adapt?

problem for ipopt

Hi, I have successfully installed ipopt, but I get error: no matching function for call to ‘corbo :: SolverIpopt :: initialize ()’ when compiling
          solver-> initialize (); // requried for setting parameters afterward

Looking forward to your reply！ thank you .

Use example get the "move_base-3" error.

Hi,Christoph Rosmann.
I use origion source and launch "roslaunch mpc_local_planner_examples carlike_minimum_time.launch" ,get [move_base-3] error in the end.
I have installed all dependencies and catkin_make successfully.I have searched "move_base-3" error for a long time but get nothing.
Could you please help me solve this issue,thank you.
PS:ubuntu 18.04 ros melodic

[ INFO] [1618374390.653249419]: Loading map from image "/home/nical/mpc_ws/src/mpc_local_planner/mpc_local_planner_examples/maps/maze.png"
[ INFO] [1618374390.687995636]: Subscribed to map topic.
[ INFO] [1618374391.097138854, 0.300000000]: Read a 200 X 200 map @ 0.050 m/cell
[ INFO] [1618374391.372478110, 0.500000000]: Received a 200 X 200 map @ 0.050 m/pix

[ INFO] [1618374391.375526743, 0.500000000]: Initializing likelihood field model; this can take some time on large maps...
[ INFO] [1618374391.380459469, 0.500000000]: Done initializing likelihood field model.
[ INFO] [1618374392.398645250, 1.600000000]: global_costmap: Using plugin "static_layer"
[ INFO] [1618374392.421153095, 1.600000000]: Requesting the map...
[ INFO] [1618374392.591438304, 1.800000000]: Resizing costmap to 200 X 200 at 0.050000 m/pix
[ INFO] [1618374392.691358487, 1.900000000]: Received a 200 X 200 map at 0.050000 m/pix
[ INFO] [1618374392.699597269, 1.900000000]: global_costmap: Using plugin "obstacle_layer"
[ INFO] [1618374392.701547553, 1.900000000]: Subscribed to Topics: laser_scan_sensor
[ INFO] [1618374392.712448318, 1.900000000]: global_costmap: Using plugin "inflation_layer"
[ INFO] [1618374392.736464387, 1.900000000]: local_costmap: Using plugin "static_layer"
[ INFO] [1618374392.738583883, 1.900000000]: Requesting the map...
[ INFO] [1618374392.739329529, 1.900000000]: Resizing static layer to 200 X 200 at 0.050000 m/pix
[ INFO] [1618374392.789361141, 2.000000000]: Received a 200 X 200 map at 0.050000 m/pix
[ INFO] [1618374392.792265019, 2.000000000]: local_costmap: Using plugin "obstacle_layer"
[ INFO] [1618374392.793613181, 2.000000000]: Subscribed to Topics: laser_scan_sensor
[ INFO] [1618374392.816575793, 2.000000000]: Created local_planner mpc_local_planner/MpcLocalPlannerROS
[ INFO] [1618374392.821068506, 2.000000000]: Footprint model 'line' (line_start: [0,0]m, line_end: [0.4,0]m) loaded for trajectory optimization.
[move_base-3] process has died [pid 7319, exit code -11, cmd /home/nical/mpc_ws/devel/lib/move_base/move_base __name:=move_base __log:=/home/nical/.ros/log/95c63164-9cd9-11eb-a37b-e45e37a829c9/move_base-3.log].
log file: /home/nical/.ros/log/95c63164-9cd9-11eb-a37b-e45e37a829c9/move_base-3*.log

Dynamic Obstacles in mpc_local_planner

Issue #55, did not get a solution. Reopening.

Inquiring about used "corbo" package

There are several packages that inherited from "corbo" has been used in mpc_local_planner package such as corbo-core, corbo-optimal-controller.

Does anyone know where to find those packages?

MPC application in an omnidirectional robot

I'm applying MPC to an omnidirectional robot, but I'm having some questions:

1- In the robot configuration file it has a function (getTwistFromControl) where it sets the robot to move in x with a certain speed u[0], according to an angle u[1]. I need to decompose the speed in vx and vy in the case of an omnidirectional robot v = sqrt(vx^2+vy^2).

I am confused how to decompose the speed coming from the controller into vx and vy

The function getTwistFromControl -> Convert the control vector to a twist message (containing velocity information)