Skip to content

LEM-RPTU/infinite_improbability_controller

BranchesTags

Repository files navigation

Infinite Improbability Controller: MPC-Based Nav2 Controller Plugin

Nav2 controller plugin based on MPC supporting static and dynamic obstacles, while having a high modularity regarding model kinematics and cost function and constraints.

Table of Contents

  1. About this repository
  2. Building and running
  3. Overview of the file structure, packages and classes
  4. Defining your own kinematics
  5. Customizing the MPC problem definition
  6. Understanding the initialisation process
  7. Related paper
  8. License
  9. Authors

About this repository

This repo provides two Nav2 controller plugins based on MPC. What is special about them and what you should be aware of while using one of the controllers:

  • special velocity smoother: The plugins are supposed to be used with our own velocity smoother, since this one can be modeled as an integrator in the MPC problem definition (without doing any approximation)
  • behavior tree (BT) node for communicating the waypoints: This repository includes a BT node, that publishes the waypoints and the controller subsribes to it. This way the controller can put a higher weight on the waypoints and therefore drive exactly through them. (Corresponding BT xml files are provided. Instructions are below.)
  • flexibility regarding kinematic (with some constraints like mendatory states)
  • modularity regarding mpc problem definition
  • obstacle avoidance
    • static obstacles as polygons
    • dynamic obstacles as circles whose position is predicted leading to expected value and covariance of their positions
  • plugin is programmed in C++, but during initalisation, it calls python scripts
  • individualisations regarding modeling the dynamics and the MPC problem definition are done by adding python scripts in the folders 'python_casadi/kinematics' and 'python_casadi/mpc_components' and setting the corresponding ROS parameters. Depending on the defined selected kinematic and MPC components, further ROS parameters are declared.
  • dependecy on casadi in C++ and python: use same release version in both (see here)
  • a second plugin without obstacle avoidance as a simple baseline for MPC related research (still in work)
  • use of casadi and continous numerical optimisation solver (e.g. IPOPT) in contrast to e.g. MPPI controller plugin

Building and running

Building

Place this repository inside the src folder of your ROS2 humble workspace (as a submodule or using git clone). Navigate into the main folder of this repo and

git submodule update --init casadi

Install the numerical sover IPOPT that we use within casadi:

sudo apt install coinor-libipopt-dev

Before bbuilding CasADi for the first time, create the file infinite_improbability_controller/casadi/colcon.pkg with the content

{
  "cmake-args": [ "-DWITH_IPOPT=ON", "-DWITH_OPENMP=ON", "-DWITH_THREAD=ON"]
}

Now navigate back into your workspace and run

colcon build

You also need CasADi for Python:

sudo apt install pip
sudo pip install casadi==3.6.7

If you want to use a newer version of CasADi, make sure that you use the same version in C++ and Python. Check the release site to know which commit matches your chosen version.

Source for CasADi installation: here

Using our controller

  • Select our controller plugin in your configuration yaml file:

    controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 2.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    progress_checker_plugin: "progress_checker"
    goal_checker_plugin: ["goal_checker"]
    current_goal_checker: ["goal_checker"]
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    # Goal checker parameters
    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25 #0.25
      yaw_goal_tolerance: 0.2
      stateful: True
    # MpcDynObstaclesController parameters
    FollowPath:
      plugin: "nav2_iic::PurePursuitIIC" # "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
      length_prediction_horizon: 15
      use_rk4: True
      use_max_cpu_time: True
      compensate_computation_time: True
      expected_computation_time: 70.0e-3
      state_const_tolerance: 1.0e-9
      desired_velocity: 0.5
      w_slag_static_obstacles: 1.0e+6
      w_k_at_waypoints: 300.0 # for the time points, at which the reference path is closest to one of the waypoints, w_pos is increased by this factor resulting in the MPC prioritizing driving through the waypoints
      max_number_obstacles: 3
      max_number_verteces_per_obstacles: 8
      sigmoid_scaling_const: 7.0

      # for differential drive:
      kinematic_type: "differential"
      kinematic_params:
        max_angular_acceleration: 0.2 #0.1
        max_angular_velocity: 0.3
        max_linear_acceleration: 0.7
        max_linear_velocity: 0.7
      # mpc_components_list: ["lqr_for_differential", "cost_on_small_turning_radius"]
      mpc_components_list: ["lqr_for_differential"]
      mpc_components_params:
        lqr_for_differential:
          w_pos: 100.0
          w_heading: 10.0
          w_lin_vel: 10.0
          w_lin_acc: 10000.0
          w_ang_acc: 500.0
        # cost_on_small_turning_radius:
        #   w_radius: 500.0

      # # for ackermann:
      # kinematic_type: "ackermann"
      # kinematic_params:
      #   axis_distance: 0.3
      #   max_angular_velocity: 0.3
      #   max_linear_acceleration: 0.7
      #   max_linear_velocity: 0.7
      #   max_steering_angle: 0.5233333333333334
      # # mpc_components_list: ["lqr_for_ackermann", "cost_on_small_turning_radius"]
      # mpc_components_list: ["lqr_for_ackermann"]
      # mpc_components_params:
      #   lqr_for_ackermann:
      #     w_pos: 100.0
      #     w_heading: 10.0
      #     w_lin_vel: 10.0
      #     w_lin_acc: 10000.0
      #   # cost_on_small_turning_radius:
      #   #   w_radius: 500.0

    Be aware, that there will exist defferent params under kinematic_params, if you set kinematic_type differently. Same for the entries within mpc_components_params if you add or remove components from mpc_components_list. Use ros2 params dump /controller_server (with the corresponding namespace of the code) during runtime or look into the corresponding files in iic_pure_pursuit/python_casadi/kinematics and iic_pure_pursuit/python_casadi/mpc_components to see the parameters. Furthermore, the default value of mpc_components_list depends on the chosen kinematic_type.

    • Select the corresponding BT xml files. You can of course hardcode the absolute file path into the parameters in the yaml, but we would recommend adapting the launch file as follows:
    nav2_iic_plugin_share = get_package_share_directory('nav2_iic_plugin')  
bt_xml_though_poses_w_publishing_waypoints = os.path.join(nav2_iic_plugin_share, 'behavior_trees', 'navigate_through_poses_w_replanning_and_recovery.xml')
bt_xml_to_pose_w_publishing_waypoints = os.path.join(nav2_iic_plugin_share, 'behavior_trees', 'navigate_to_pose_w_replanning_and_recovery.xml')

nav2_config_file = ReplaceString(
    source_file=params_file,
    replacements={'<robot_namespace>': ('/', current_namespace),
                    'default_nav_through_poses_bt_xml:': 'default_nav_through_poses_bt_xml: "'+bt_xml_though_poses_w_publishing_waypoints+'"',
                    'default_nav_to_pose_bt_xml:': 'default_nav_to_pose_bt_xml: "'+bt_xml_to_pose_w_publishing_waypoints+'"'},
)

param_substitutions = {
    'use_sim_time': use_sim_time}

configured_params = ParameterFile(
    RewrittenYaml(
        source_file=nav2_config_file,
        root_key=current_namespace,
        param_rewrites=param_substitutions,
        convert_types=True),
    allow_substs=True)

    For this to work, your yaml needs the entries

    bt_navigator:
  ros__parameters:
    default_nav_through_poses_bt_xml:
    default_nav_to_pose_bt_xml:

    so that they can be overwritten by ReplaceString(). params_file is the file path to your original yaml (or a corresponding LaunchConfiguration) and configured_params is what you give to all the nodes using parameters=[configured_params],.

  • Use our own velocity smoother instead of the default one: Comment out the default velocity_smoother_node in your launch file and replace it with

    velocity_smoother_node = Node(
    package='nav2_iic_velocity_smoother',
    executable='velocity_smoother_integrator',
    output='screen',
    namespace=current_namespace,
    parameters = [configured_params,
                    {'params_file': params_file}],
    arguments=['--ros-args', '--log-level', log_level],
    remappings=[
        ('/tf', 'tf'),
        ('/tf_static', 'tf_static')
        ]
)

    The default velocity smoother is a lifecycle node. Our own velocity smoother is (until now) only a simple Python node. Because of that, you have to comment out the entry 'velocity_smoother' in the list node_names (a ROS parameter of lifecycle_manager_node, that is typically set in the launch file.)

    In the longterm, it hopefully gets replaced by a lifecycle node with the same interface (topic names, executable name, etc.) like the standard velocity smoother, so that you only have to replace package='nav2_velocity_smoother' by package='nav2_iic_velocity_smoother'.

    Currently our velocity smoother gets the path to the yaml configuration file as a parameter and reads there the parameter controller_frequency of the node controller_server. In the long term, it should periodically request the controller frequency using a service call (using multithreaded executor and separate callback groups and mutex on the stored controller_frequency) or measure the duration between receiving messages internally.

Overview of the file structure, packages and classes

The current repository looks like

infinite_improbability_controller
├── casadi
├── iic_pure_pursuit
│   ├── CMakeLists.txt
│   ├── config
│   │   └── default_params.yaml
│   ├── iic_pure_pursuit.xml
│   ├── include
│   │   └── iic_pure_pursuit
│   │       ├── infinite_improbability_controller.hpp
│   │       ├── kinematic_info.hpp
│   │       ├── mpc_problem.hpp
│   │       ├── parameter_handler.hpp
│   │       ├── polygon_obstacles.hpp
│   │       ├── pure_pursuit_iic.hpp
│   │       └── references_to_mpc.hpp
│   ├── LICENSE
│   ├── package.xml
│   ├── python_casadi
│   │   ├── casadi_utility.py
│   │   ├── create_mpc_problem.py
│   │   ├── __init__.py
│   │   ├── kinematics
│   │   │   ├── ackermann.py
│   │   │   ├── differential.py
│   │   │   ├── __init__.py
│   │   │   └── kinematic.py
│   │   ├── mpc_components
│   │   │   ├── cost_on_small_turning_radius.py
│   │   │   ├── __init__.py
│   │   │   ├── lqr_for_ackermann.py
│   │   │   ├── lqr_for_differential.py
│   │   │   └── mpc_components_utility.py
│   │   ├── mpc_problem_definition.casadi
│   │   ├── plot_ackermann.py
│   │   ├── plot_differential.py
│   │   └── README.md
│   ├── README.md
│   └── src
│       ├── infinite_improbability_controller.cpp
│       ├── kinematic_info.cpp
│       ├── mpc_problem.cpp
│       ├── parameter_handler.cpp
│       ├── polygon_obstacles.cpp
│       ├── pure_pursuit_iic.cpp
│       └── references_to_mpc.cpp
├── nav2_iic_interfaces
│   ├── CMakeLists.txt
│   ├── LICENSE
│   ├── package.xml
│   └── srv
│       └── GiveWaypointsToMpc.srv
├── nav2_iic_plugin
│   ├── behavior_trees
│   │   ├── navigate_through_poses_w_replanning_and_recovery.xml
│   │   └── navigate_to_pose_w_replanning_and_recovery.xml
│   ├── CMakeLists.txt
│   ├── include
│   │   └── nav2_iic_plugin
│   │       └── bt_waypoint_pub.hpp
│   ├── LICENSE
│   ├── package.xml
│   └── src
│       └── bt_waypoint_pub.cpp
├── nav2_iic_velocity_smoother
│   ├── LICENSE
│   ├── nav2_iic_velocity_smoother
│   │   ├── __init__.py
│   │   └── vel_smoother_integrator.py
│   ├── package.xml
│   ├── resource
│   │   └── nav2_iic_velocity_smoother
│   ├── setup.cfg
│   ├── setup.py
│   └── test
│       ├── test_copyright.py
│       ├── test_flake8.py
│       └── test_pep257.py
└── README.md

What's important about the file structure:

  • casadi: Submodule for modeling the optimization problem
  • iic_pure_pursuit: package with our controller plugin
  • iic_pure_pursuit/python_casadi/kinematics: here you define your own kinematic model (if you want)
  • iic_pure_pursuit/python_casadi/mpc_components: here you define your own parts of the MPC problem definition (if you want)
  • nav2_iic_interfaces: package that defines the interface (service type) that we use to send the waypoints from the BT node to the controller plugin
  • nav2_iic_plugin: package that includes the BT node to send the waypoints to the controller plugin
  • nav2_iic_velocity_smoother: package, that includes our own velocity smoother (that can be easily modeled in the MPC formulation in contrast to the default velocity smoother)

Important classes:

  • PurePursuitIIC: actual controller plugin (inherits from nav2_core::Controller)
  • ReferencesToMPC: calculates all the references, that are given to the MPC solver
  • PolygonObstacles: includes the costmap_converter, does the pre-processing of the polygons and generates the matrices that the MPC solver needs
  • KinematicInformation: holds informations about the kinematic model, the time discrete state space model itself and the kinematic parameters. It includes methods to call python scripts for exporting these informations and methods for loading them from the exported files.
  • ParameterHandler: holds parameters and includes methods for declaring them and getting their values
  • MPCproblem: hold the solver and some variables, that are needed for the next cycle. It calls python for exporting the solver and loads the exported file. It simplifies calling the solver.

Defining your own kinematics

If the predefined kinematics don't match your robot, you can define your own kinematics, by creating a new python script inside iic_pure_pursuit/python_casadi/kinematics. Use e.g. ackermann.py or differential.py as a template. You select the kinematic by setting the ROS parameter kinematic_type. This always corresponds to the filename without .py. The names of the functions have to be the same as in e.g. ackermann.py. Regarding these functions:

  • get_kinematic_information(): The entry name has to fit the file name. State, input and parameters are vectors. The meaning of their entries are defined in state_index, input_index and parameter_index. Mandatory entries of the state are x, y (position) and heading_angle. The parameters, you define here are all float values and specific to the kinematic. They can be used for defining the dynamic model of the robot (like e.g. axis_distance) and/or for defining kinematic specific constraints (like e.g. max_steering_angle and axis_distance). Corresponding ROS parameters (with default value and description) are automatically defined during initialisation. You can furthermore define the default value of the ROS parameter mpc_components_list for the case, that this kinematic is used.
  • get_time_discrete_model(): Here you define the kinematic/dynamic model.Typically you first define the time continuous state space model and then discretise it. These objects of type casadi.Function have to have a predefined list of inputs and outputs. Therefore even if the model doesn't depend on the parameters, they still have to be part of the inputs to keep the standard form of casadi.Function.
  • add_kinematic_specific_constraints(): Here you can add constraints, that are specific to the kinematic (e.g. for ackermann you want to limit the state steering_angle, that doesn't exist for other kinematics. For differential you want to limit the input angular_acceleration, that doesn't exist for e.g. ackermann). These constraints are always given in the form of glb < g < gub, where all three of these variables are vectors and the inequality constraints are understood line by line. Use float("inf") or float("-inf") if you only need one limit (only lower or only upper bound). Consider indroducing a tolerance while expressing an equality constraint through an inequality constraint for numerical performance. Only g (not glb or gub) is allowed to include symbolic expressions (states and inputs) the way it is currently implemented. Reformulate the inequalities accordingly. Remenber that the states are defined from step 0 to step N and inputs only from 0 to N-1. Putting constraints on expressions that only depent on the initial state makes no sense and can lead to infeasability. Consider these point while defining the range of time steps in the for loops.
  • get_cmd_vel_as_dict(): After the optimization problem is solved, you need to calculate the message cmd_vel (ROS2 message type geometry_msgs::msg::TwistStamped) based on the result of the optimisation problem. How this is done for the different kinematics, is defined here. Typically (because how our velocity smoother works) you return the velocity of time step 1 (not acceleration of time step 0).

While defining the state space model and how the cmd_vel is calculated, keep in mind how our velocity smoother works. It interpolates linearly between the last 2 cmd_vel messages that it received.

Customizing the MPC problem definition

You can extend the MPC formulation by writing a mpc_component, by creating a new python script inside iic_pure_pursuit/python_casadi/mpc_components. You can add terms to the cost function or add additional constraints. Only soft constraints are currently not possible, since you would have to introduce a new decision variable which is currently not possible. Use e.g. lqr_for_differential.py or lqr_for_ackermann.py as a template. You select the components to use by setting the ROS parameter mpc_components_list. The entries in this list always correspond to the filenames without .py. The names of the functions have to be the same as in e.g. lqr_for_differential.py. Regarding these functions:

  • only make changes in the 2 sections, that are marked with comments.
  • get_additional_params(): For most constraints and terms of the cost function, you need parameters like limits and weights. These are defined here. Corresponding ROS parameters (with default value and description) are automatically defined during initialisation.
  • appand_to_mpc_definition(): Here you expand the cost function and define additional constraints. Often these mpc_components are not compatible with all kinematics, especially if they depend on anything more than the mandatory states x, y (position) and heading_angle. Read what I already wrote regarding the function add_kinematic_specific_constraints(). It applies also here. glb and gub may not depend on anything of inputs_for_creating_mpc_components, but may depend on kinematic_params_as_dict.

Understanding the initialization process

The initialization of the controller plugin happens inside the method PurePursuitIIC::configure inside iic_pure_pursuit/src/pure_pursuit_iic.cpp and is a bit complex. Some processes include calling python scripts, that export files that are again loaded in C++. Many processes depend on the outcome of previous processes. These dependencies dictate the order of executing the individual steps and are depicted in the following diagram. The steps of the initialisation are the nodes and the arrows are the dependencies

---
config:
  theme: redux
  layout: dagre
---
flowchart TD
    A(["Start"]) --> n1["Declaring and loading the values<br>of the general ROS parameters"]
    n1 -- knowing the parameter<br>'kinematic_type' --> n2["calling python to export<br>KinematicInformation as json"]
    n2 --> n3["loading KinematicInformation<br>from json file"]
    n3 -- knowing which kinematic<br>specific parameters<br>exist --> n4["declaring and loading<br>kinematic specific<br>parameters as <br>ROS parameters"]
    n3 -- knowing default<br>value of<br>'mpc_components_list' --> n5@{ label: "declare and load<br>ROS parameter<br>'mpc_components_list'" }
    n4 -- knowing values of<br>kinematic specific<br>parameter --> n6["saving these kinematic<br>parameter as casadi::DM<br>inside the kinematicInfo"]
    n5 -- knowing which<br>mpc_components<br>will be used --> n7["call python to export file<br>with info about component<br>specific parameters"]
    n7 --> n8["loading this file and<br>declaring and loading<br>corresponding ROS parameter"]
    n6 -- knowing kinematic_type<br>and kinematic specific<br>parameters (which exist<br>and their values) --> n9["call python to<br>export casadi::Function<br>that calls the solver,<br>seperates the result,<br>determines cmd_vel, ..."]
    n8 -- knowing<br>mpc_components_list<br>and component specific<br>parameters (which exist<br>and their values) --> n9
    n1 -- "<span style=padding-left:>knowing the parameter<br>'kinematic_type'</span>" --> n10["call python to export<br>time discrete model<br>of kinematics"]
    n10 --> n11["loading kinematic<br>model as<br>casadi::Function"]
    n9 --> n12["load this<br>casadi::Function"]
    n1@{ shape: rect}
    n2@{ shape: rect}
    n3@{ shape: rect}
    n4@{ shape: rect}
    n5@{ shape: rect}
    n6@{ shape: rect}
    n7@{ shape: rect}
    n8@{ shape: rect}
    n9@{ shape: rect}
    n10@{ shape: rect}
    n11@{ shape: rect}
    n12@{ shape: rect}
Loading

(Diagram made with https://www.mermaidchart.com/play. Install a VScode mermaid extension to view it.)

Related paper

License

This software is licensed under the BSD-3-Clause license.

Please note that this project depends on third-party libraries licensed under:

  • Apache License 2.0
    • rclcpp
    • rclpy
    • common_interfaces
    • rcl_interfaces
    • ament_index_python
    • geometry_msgs
    • visualization_msgsc
    • ament_lint_auto
    • ament_lint_common
    • ament_copyright
    • ament_flake8
    • ament_pep257
    • ament_cmake_python
    • rosidl_default_generators
    • rosidl_default_runtime
    • rosidl_interface_packages
    • nav2 stack in parts
  • BSD
    • costmap_converter
  • BSD 3-Clause License
    • nav2 stack in parts
    • pluginlib
    • geometry2
  • LGPL-3.0
    • casadi
  • MIT
    • nlohmann_json
    • python3-pytest
  • LGPL-2.1
    • nav2 stack in parts

Casadi requires dynamic linking due to LGPL conditions, hence casadi is linked via git submodule.

Numpy has a custom license which you can find here:

Copyright (c) 2005-2025, NumPy Developers. All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright
   notice, this list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above
   copyright notice, this list of conditions and the following
   disclaimer in the documentation and/or other materials provided
   with the distribution.

* Neither the name of the NumPy Developers nor the names of any
   contributors may be used to endorse or promote products derived
   from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

Authors:

About

No description, website, or topics provided.

Resources

Readme

License

BSD-3-Clause license
Activity
Custom properties

Stars

1 star

Watchers

0 watching

Forks

0 forks
Report repository

Releases

No releases published

Packages

No packages published

Contributors 4

  •  
  •  
  •  
  •  

Languages