Working with multiple dynamically allocated controllers

[gacamilo]

Hi @nicolapiccinelli I think this might be more of a question than an issue... (Unfortunately, I am no cpp guru) I've been having trouble instantiating multiple LMPC objects for problems of different dimensions, when I run the lines below the LMPC template captures the problem dimensions of the last statement only. For example when I try to set the matrices for latController, I get Eigen resize errors on setStateSpaceModel because the controller instance thinks that the dimensions of the problem are those of yawController instead of latController. Is there a way to instantiate multiple dynamically allocated problems like I'm trying to do? I tried the static allocation but my matrices are too big.

// MPC LATERAL CONTROLLER
mpc::LMPC<8, 2, 2, 5, 50, 3> latController;
auto latCtrlInput = latController.getLastResult();

// MPC LONGITUDINAL CONTROLLER
mpc::LMPC<8, 2, 2, 5, 50, 3> longController;
auto longCtrlInput = longController.getLastResult();

// MPC VERTICAL CONTROLLER
mpc::LMPC<4, 1, 1, 3, 50, 3> vertController;
auto vertCtrlInput = vertController.getLastResult();

// MPC YAW CONTROLLER
mpc::LMPC<4, 1, 1, 2, 50, 3> yawController;
auto yawCtrlInput = yawController.getLastResult();

[nicolapiccinelli]

@camilogonzalez97 can you provide an MWE so I can test it

[gacamilo]

The lines below are enough to reproduce the error.

#include <mpc/LMPC.hpp>
#include <Eigen/Dense>

// Define two controllers with different dimensions
mpc::LMPC<> firstController(2, 1, 1, 1, 20, 3);
mpc::LMPC<> sencondController(3, 1, 1, 2, 20, 3);

int main() {
    // Initialize first controller model
    Eigen::MatrixXd A(2, 2), B(2, 1), C(1, 2);
    A << -2, 1, -9.773, -1.386;
    B << 0, 1;
    C << 0.5, 0;
    firstController.setStateSpaceModel(A, B, C);

    /*Does not work because firstController thinks that the dimensions of the 
        SS model are the ones specified for secondController */
}

Inside setStateSpaceModel the method ProblemBuilder::setStateModel throws an Eigen resize error at [this line](https://github.com/nicolapiccinelli/libmpc/blob/master/include/mpc/ProblemBuilder.hpp#:~:text=ssA.block(0%2C%200%2C%20dim.nx.num()%2C%20dim.nx.num())%20%3D%20A%3B) because dim.nx.num() is 3 instead of 2.

[nicolapiccinelli]

@camilogonzalez97 sorry for the late reply. I gave a look but as it is now there’s no way to have multiple mpcs in the same executable. You can try to split your controller in different nodes (if you are using ROS)

[gacamilo]

No worries. I assumed it wasn't possible and ended up making my own barebones implementation. It is completely based on your library. I'm attaching the main file here in case someone needs it in the future. FastLMPC.zip

I optimised everything I could for speed including making the problem constant and omitting osqp_setup on all iterations other than the first one.

You can use it like this:

// Instantiate
FastLMPC controller(_, _, _, _, _);

// Configure
controller.setOptimizerParams(_);
controller.setStateSpaceModel(_, _, _);
controller.setDisturbances(_, _);
controller.setObjectiveWeights(_, _, _);
controller.setConstraints(_, _, _, _, _, _);
controller.initProblem();

// Solve repeatedly
controller.solve(_, _, _, _, _, _);

[nicolapiccinelli]

I will give a look into your implementation and maybe we can include some other updates in the main trunk.

[nicolapiccinelli]

@camilogonzalez97 take a look to the latest release 0.0.9 it should allows you to have multiple mpc instances.

Cheers!

[gacamilo]

Nice! The library is looking great @nicolapiccinelli