ROS2 controller

In the previous section, we used Acados to define an optimal control problem (OCP) and solver with Acados.

In this section, we will show how to generate a solver for C++ ROS2 controllers and how to use it.

Export the solver plugin

To use the Acados solver in a C++ ROS2 controller, we first need to export the solver as a pluginlib plugin. The Python module acados_solver_plugins provides a class SolverPluginGenerator to generate the plugin:

class acados_solver_plugins.solver_plugin_generator.SolverPluginGenerator(custom_export_path: str | None = None, library_name: str | None = None)[source]

Minimalist template-based code generation module to generate acados::AcadosSolver derived objects.

Relevant external API documentation : jinja2, acados_template.

_generate_file_from_template(template_filename, export_filename, **template_render_args)[source]
generate_solver_plugin(acados_ocp: acados_template.AcadosOcp, plugin_class_name: str, solver_description: str | None = None, x_index_map: dict | None = None, z_index_map: dict | None = None, p_index_map: dict | None = None, u_index_map: dict | None = None) → acados_template.AcadosOcpSolver[source]

Generate the solver plugin from a python Acados OCP model.

Parameters:

  • acados_ocp (AcadosOcp) – Acados OCP model

  • plugin_class_name (str) – Class name for the generated plugin of type “acados::<plugin_class_name>”

  • solver_description (str, optional) – Short description for the exported plugin, defaults to None

  • x_index_map (dict, optional) – Differential state index map, defaults to None

  • z_index_map (dict, optional) – Algebraic state index map, defaults to None

  • p_index_map (dict, optional) – Parameters index map, defaults to None

  • u_index_map (dict, optional) – Parameters index map, defaults to None

Returns:

the acados OCP solver generated as a by-product

Return type:

AcadosOcpSolver

jinga_env_loader

Jinja environment loader initialized as a jinja2.PackageLoader looking for templates in the installation folder of the module.

Note that the generate_solver_plugin method of the SolverPluginGenerator class takes semantic maps as arguments, mapping variable names to their indices in the state, control, and parameter vectors.

For the RRBot example, the export script is as follows:

model = AcadosModel()
...  # define the model (see previous section)
ocp = AcadosOcp()
...  # define the OCP (see previous section)

# define the semantic map of the OCP variables
x_index_map = {
    'q': [0, 1],
    'q_dot': [2, 3],
}
z_index_map = {}  # N.B., actually not empty in demo code
p_index_map = {
    'l0': [0],
    'l1': [1],
    'l2': [2],
    'm1': [3],
    'm2': [4],
    'p_ref': [5, 6],
    'p_dot_ref': [7, 8],
    'Q_pos_diag': [9, 10],
    'Q_vel_diag': [11, 12],
    'R_diag': [13, 14],
}
u_index_map = {
    'tau': [0, 1],
}


# export the solver plugin
from acados_solver_plugins import SolverPluginGenerator

solver_plugin_generator = SolverPluginGenerator()

solver_plugin_generator.generate_solver_plugin(
    ocp=ocp,
    plugin_class_name='RrbotCartesianTracking',
    solver_description='Acados solver plugin to track a Cartesian trajectory with the RRBot planar robot',  # noqa: E501
    x_index_map=x_index_map,
    z_index_map=z_index_map,
    p_index_map=p_index_map,
    u_index_map=u_index_map,
)

The full code can be found in the export_acados_solver_plugin.py file in the example_acados_controller/script directory of the acados_solver_ros2_examples repository.

To export the plugin, simply run the script with Python:

source install/setup.bash
python3 ./<relative_path_to_script>.py

Which results in the following workspace tree:

../_images/workspace_tree.png

As shown in the figure, the plugin sources are exported to the src directory of the acados_solver_plugins package.

The wrapper inherits from the acados::AcadosSolver class, resulting in the following pluginlib XML config (i.e., the export_plugin.xml file):

<library path="acados_solver_plugins">
<class name="acados_solver_plugins/RrbotCartesianTracking"
        type="acados_solver_plugins::RrbotCartesianTracking"
        base_class_type="acados::AcadosSolver">
    <description>
    Acados solver plugin to track a Cartesian trajectory with the RRBot planar robot
    </description>
</class>
</library>

In order to use the plugin in a C++ ROS2 controller, we will need to rebuild the acados_solver_plugins package:

source install/setup.bash
colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install

Use the plugin in a C++ controller

1) Setup the ROS2 controller package dependencies

Add the acados_solver_base and acados_solver_plugins packages as dependencies of the ROS2 controller package :

<!-- In package.xml>

...
<build_depend>acados_solver_base</build_depend>

<exec_depend>acados_solver_base</exec_depend>
<exec_depend>acados_solver_plugins</exec_depend>
...

# In CMakeLists.txt
# ...
find_package(acados_solver_base REQUIRED)
# ...
ament_target_dependencies(
    example_acados_controller
    PUBLIC
        # ...
        acados_solver_base
)

The full code can be found in the package.xml and CMakeLists.txt files in the example_acados_controller directory of the acados_solver_ros2_examples repository.

2) Load the solver plugin in the ROS2 controller

Define a ROS2 controller with a pluginlib loader to load the solver plugin:

// In example_acados_controller.hpp

// ...
#include "acados_solver_base/acados_solver_base.hpp"
#include "pluginlib/class_loader.hpp"

namespace example_acados_controller
{

class ExampleAcadosController : public controller_interface::ControllerInterface
public:
    ExampleAcadosController();

    CallbackReturn on_init() override;

    controller_interface::InterfaceConfiguration command_interface_configuration() const override;

    controller_interface::InterfaceConfiguration state_interface_configuration() const override;

    CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;

    // ...

    controller_interface::return_type update(
        const rclcpp::Time & time,
        const rclcpp::Duration & period) override;

protected:
    // ...

    /// Acados solver pluginlib loader
    std::shared_ptr<pluginlib::ClassLoader<acados::AcadosSolver>> acados_solver_loader_;

    /// Acados solver
    std::unique_ptr<acados::AcadosSolver> acados_solver_;

    // ...
};

}  // namespace example_acados_controller

The full code can be found in the example_acados_controller.hpp files in the example_acados_controller/include directory of the acados_solver_ros2_examples repository.

3) Configure the solver plugin in the ROS2 controller

The solver plugin can be loaded and configured in the on_configure method of the ROS2 controller. Additionally, the solver should be reset in the on_activate method (not shown here). Note that the solver settings (e.g., the number of shooting nodes and the sampling time) can be changed at runtime:

// In example_acados_controller.cpp

#include "example_acados_controller/example_acados_controller.hpp"

// ...

namespace example_acados_controller
{

// ...

CallbackReturn
ExampleAcadosController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
{
    // ...

    // get solver settings
    std::string nmpc_plugin_name = "acados_solver_plugins/RrbotCartesianTracking";
    int N = 10; // number of shooting nodes (i.e., the prediction horizon)
    double Ts = 0.002; // 500 Hz

    // load the solver plugin
    acados_solver_loader_ =
        std::make_shared<pluginlib::ClassLoader<acados::AcadosSolver>>(
        "acados_solver_base", "acados::AcadosSolver");
    acados_solver_ = std::unique_ptr<acados::AcadosSolver>(
        acados_solver_loader_->createUnmanagedInstance(nmpc_plugin_name));

    // configure the solver
    if (0 != acados_solver_->init(N, Ts)) {
        RCLCPP_ERROR(
            get_node()->get_logger(), "Failed to configure the Acados solver!");
        return CallbackReturn::FAILURE;
    }

    // ...
}

// ...

}  // namespace example_acados_controller

The full code can be found in the example_acados_controller.cpp files in the example_acados_controller/src directory of the acados_solver_ros2_examples repository.

3) Solve the OCP

The OCP can be solved in the update method of the ROS2 controller:

// In example_acados_controller.cpp

// ...

namespace example_acados_controller
{

// ...

controller_interface::return_type
ExampleAcadosController::update(
    const rclcpp::Time & time,
    const rclcpp::Duration & period)
{
    // get runtime parameters
    std::vector<double> Q_pos_diag = {0.0, 0.0};
    std::vector<double> Q_vel_diag = {0.0, 0.0};
    std::vector<double> R_diag = {0.0, 0.0};
    double l0 = 0.0;
    // ...

    // get the current joint state
    std::vector<double> q_pos = {0.0, 0.0};
    std::vector<double> q_vel = {0.0, 0.0};

    // set the OCP initial state
    acados::ValueMap x_values_map;
    x_values_map["q"] = q_pos;
    x_values_map["q_dot"] = q_vel;

    if (0 != acados_solver_->set_initial_state_values(x_values_map)) {
        RCLCPP_ERROR(
            get_node()->get_logger(), "Failed to set NMPC initial state values!");
        return controller_interface::return_type::ERROR;
    }

    // get the Cartesian reference
    std::vector<double> p_ref = {0.0, 0.0};
    std::vector<double> p_dot_ref = {0.0, 0.0};

    // set the OCP parameters
    acados::ValueMap p_values_map;
    p_values_map["l0"] = std::vector{l0};
    p_values_map["l1"] = std::vector{l1};
    p_values_map["l2"] = std::vector{l2};
    p_values_map["m1"] = std::vector{m1};
    p_values_map["m2"] = std::vector{m2};
    p_values_map["p_ref"] = p_ref;
    p_values_map["p_dot_ref"] = p_dot_ref;
    p_values_map["Q_pos_diag"] = Q_pos_diag;
    p_values_map["Q_vel_diag"] = Q_vel_diag;
    p_values_map["R_diag"] = R_diag;

    if (0 != acados_solver_->set_runtime_parameters(p_values_map)) {
        RCLCPP_ERROR(
            get_node()->get_logger(), "Failed to set NMPC runtime parameters!");
        return controller_interface::return_type::ERROR;
    }

    // solve the OCP
    bool all_ok = true;
    std::vector<double> tau_cmd = {0.0, 0.0};
    if (0 != acados_solver_->solve()) {
        RCLCPP_ERROR(get_node()->get_logger(), "Failed to solve the NMPC SQP problem!");
        all_ok = false;
    } else {
        // Get optimal control input
        acados::ValueMap u_values_map = acados_solver_->get_control_values_as_map(0);
        tau_cmd[0] = u_values_map["tau"][0];
        tau_cmd[1] = u_values_map["tau"][1];
    }

    // Send command to robot
    // ...
}
// ...

}  // namespace example_acados_controller

The full code can be found in the example_acados_controller.cpp files in the example_acados_controller/src directory of the acados_solver_ros2_examples repository.

Notes and recommendations

  • If not provided, the semantic map is constructed from CasADI variable names.

  • By default, the plugin sources are exported to the src directory of the acados_solver_plugins package. This can be changed by setting the plugin_sources_dir argument of the generate_solver_plugin method.

  • If the Acados solver C library is generated for the first time, the user will be prompted to install some Acados external dependencies. This can be avoided by running the script as follows:

    echo "y" | python3 ./<relative_path_to_script>.py

  • The ValueMap class is a simple wrapper around a std::map to store the values of the OCP variables. It is a convenient way to pass the values to the Acados solver, but it is not mandatory. The user can directly set the values from a std::vector object, which might be more efficient as the ValueMap setters are not especially optimized.