implementin NMPC in ROS using code generation

Is it possible to to use Matlab code generation to implement a ROS node for NMPC?

I have defined a nlmpc object and I am perfoming closed-loop simulations in Matlab using the nlmpcmoveCodeGeneration() command after having created the coreData and onlineData struct using getCodeGenerationData().

Does anyone know if it is possible to implement a ROS node using code generation to import this same controller to ROS envionment? The reason behind this is that I have designed a controller using Matlab simulations, and I would now like to test the controller with real hardware that is operating with ROS.

Currently I am having the following error message when trying to use the code generation:

----------

>>

Non-constant expression or empty matrix. This expression must be constant because its value determines the size or class of some expression.

Error in ==> nlmpcmoveCodeGeneration Line: 265 Column: 20

Code generation failed: View Error Report

Error using codegen

----------

I am using the following script for the code generation. I am using Matlab version 2023a on Ubuntu 20.04

cfg = coder.config("exe");
cfg.Hardware = coder.hardware("Robot Operating System (ROS)");
cfg.Hardware.DeployTo = "Localhost"; 
cfg.Hardware.BuildAction = "Build and run";
Cfg.DynamicMemoryAllocation = 'off';
cfg.HardwareImplementation.ProdLongLongMode = true;
codegen myTestnode -config cfg

Please let me know if someone has any experience with similiar problems with NMPC or if somone has any suggestions on how to proceed with this.

Thank you!

[Josh Chen]

hey Mikael,

The error message seems to point out an mismatch inside the "nlmpcmoveCodeGeneration" function. Is there a sample code you can share to reproduce this?

Thanks,

Josh

[Mikael_P]

Hi Josh,

The model that I am using is quite complex and contains custom cost and output functions that are defined in separate functions as suggested in the documentation. The sricpt that I use to test the ROS node generation is attaced here. I have set the structures coreData and onlineData as global variables so that these are found within the function. Then I have defined two general ROS subscribes to get the current state information from the system.

In the actual implementation, the state information is supposed to come from a seperate EKF node. The state vector of the system consists of 7 state variables so I have just assigned the vector Xc to include seven states from the those general subscribers. Then I used the script presented in the original post to generate the ROS node from this function presented here.

Please let me know is you need some additional information. I can also provide the whole system model and custom functions if needed, but they are quite complex. Thank you!

function myTestnode()
    %#codegen
    global coreData onlineData
    % get state infromation
    sub1 = rossubscriber("/state1","geometry_msgs/Vector3","DataFormat","struct");
    sub2 = rossubscriber("/state2","geometry_msgs/Vector3","DataFormat","struct");
    
    while (1) 
        msg_get1 = receive(sub1);
        msg_get2 = receive(sub2);
        U=[msg_get1.X; msg_get2.X];
        %msg_get2 = receive(sub2);
        if ~isempty(msg_get1.X > 0)
            %create state vector Xc from messages:
            Xc=[msg_get1.X; msg_get1.Y; msg_get1.Z; msg_get2.X; msg_get2.Y; msg_get2.Z; msg_get1.X]; 
            U=[msg_get1.X; msg_get2.X];
            yref = zeros(1,5);
            yref(1) = msg_get1.X; yref(2)=msg_get2.X
            onlineData.ref=yref;
            [U,onlineData] = nlmpcmoveCodeGeneration(coreData,Xc,U,onlineData);
        else
           fprintf("Received empty message..\n");
        end
    end
end

[Josh Chen]

Hi Mikael,

This simplified function looks like a good point to start. Could you please also provide a dummy "coreData" and "onlineData" so that I can try this out on my end and see where it failed?

One thing I observe in this script is "~isempty(msg_get1.X>0)", I believe this will always return "true". If you would like to only run the code when receiving a message with X larger than 0, you can remove "~isempty()".

Thanks,

Josh

[Mikael_P]

Hi Josh,

I have now created a simplified test model that produces the same error inside the "nlmpcmoveCodeGeneration" function when tring to create the ROS node. In this simplified model the number of states is 6, number of inputs is 2 and number of outputs is 3.

With this model the myTestnode() function that I try to convert to ROS is defined as:

function myTestnode()
    %#codegen
    global coreData onlineData
    % Trajectory points to publish
    sub1 = rossubscriber("/state1","geometry_msgs/Vector3","DataFormat","struct");
    sub2 = rossubscriber("/state2","geometry_msgs/Vector3","DataFormat","struct");
    U=[0; 0]; %intial control command
    while (1) 
        msg_get1 = receive(sub1);
        msg_get2 = receive(sub2);
        U=[msg_get1.X; msg_get2.X];
        if msg_get1.X > 0
        %create state vector Xc from messages:
        Xc=[msg_get1.X; msg_get1.Y; msg_get1.Z; msg_get2.X; msg_get2.Y; msg_get2.Z]; 
        %define references
        yref = zeros(1,3);
        yref(1) = 1; yref(2)=0;
        onlineData.ref=yref;
        [U,onlineData] = nlmpcmoveCodeGeneration(coreData,Xc,U,onlineData);
        else
           fprintf("Received empty message..\n");
        end
    end
end

The global structures coreData and onlineData are created with the following script, that defines the prediction models and constraints using custom functions.

%define number of states, outputs, inputs 
nx = 6;
ny = 3;
nu = 2;
nlobj = nlmpc(nx,ny,nu);

%set sampling time, prediction- and control horizon
Ts=0.1;
nlobj.Ts = Ts;
nlobj.PredictionHorizon = 21;
nlobj.ControlHorizon = 21;

%define state space model
nlobj.Model.StateFcn = "TestModelFnc";
nlobj.Model.IsContinuousTime = true;
nlobj.Model.NumberOfParameters = 1;

%define custom ouput fnc
nlobj.Model.OutputFcn='TestModelOutputFnc';

nlobj.Optimization.CustomCostFcn = 'TestCustomCostFnc';
nlobj.Optimization.ReplaceStandardCost=true;


%define state constraints
nlobj.States(2).Min=-0.2625; % curvature
nlobj.States(2).Max=0.2625;

nlobj.States(4).Min=0;

nlobj.States(6).Min=0;

%define input constraints
nlobj.ManipulatedVariables(1).Min=-0.5;
nlobj.ManipulatedVariables(1).Max=0.5;

nlobj.ManipulatedVariables(2).Min=-5.25;
nlobj.ManipulatedVariables(2).Max=5.25;

%define output constraints
nlobj.OV(3).Min=-3; % lateral acceleration
nlobj.OV(3).Max=3;

%create coreData & onlineData structures
Xc0=zeros(6,1);
U=zeros(2,1);
global coreData onlineData
params = {Ts};
[coreData,onlineData] = getCodeGenerationData(nlobj,Xc0,U,params);
onlineData.md=0;

The custom functions used to define the prediction model, outptut functions and cost functions are defined as:

function dxdt = TestModelFnc(X, U, params)
% state variables
x = X(1);
y = X(2);
hdg = X(3);
V = X(4);
curv = X(5);
dist = X(6);
% inputs
acmd=U(1);
dKcmd = U(2);

%state equations
dxdt(1) = V*cos(hdg);
dxdt(2) = V*sin(hdg);
dxdt(3) = V*Kcmd;
dxdt(4) = acmd; 
dxdt(5) = dKcmd; 
dxdt(6) = V;

end

function J = TestCustomCostFnc(X,U,e,data, params)
    [Np, Ns]=size(X); % Np = prediction horizon + 1
    Np=Np-1;

    %state references:
    x_ref=zeros(Np,Ns);
    x_ref(:,1)=data.References(:,1);
    x_ref(:,2)=data.References(:,2);

    %state reference weights
    q=zeros(1,Ns);
    q(4)=5;
    q(5)=5;
    Q=diag(q);
    
    %input weights
    r=[2.5 1];
    R=diag(r);
    %input ref
    U_ref=[0 0];

    J = 0;
    for i =[1:Np]  
        cost_X=(X(i+1,:)-x_ref(i,:))*Q*(X(i+1,:)-x_ref(i,:))'; %cost from the states
        cost_U=(U(i+1,:)-U_ref)*R*(U(i+1,:)-U_ref)'; %cost from the inputs
        J=J+cost_X+cost_U;    
    end
end

function Out=TestModelOutputFnc(X,U, params)
    % define model outputs
    V=X(4);
    curv=X(5);
    a_lat=curv*V^2;
    Out=[V; curv; a_lat];
end

So by defining the controller using these scripts and functions I get the exact same error message as mentioned in the original post. I don't know if it is a generally good idea to use global variables in the code generation of ROS node, but it was the simpliest solution that I have came up. Also the model presented here is only a general model that I used to define the coreData and onlineData structures that are used in the test node generation.

Hopefully you are able to enlighten me on what is causing the problems and is it even possible to generate ROS node for NMPC using this approach. Thank you!

Answer 1

[Josh Chen]

Hi Mikael,

Thanks for sharing this additional information. I think using "goal variable" is a smart idea for generating the ROS node with mpc. The error message you saw earlier indicates that the first argument for nlmpcmoveCodeGeneration need to be constant. Based on the script you provided, it seems you are not planning to change the value of "coreData" anyway.

To let MATLAB coder understand this global variable will be a constant, you can specify it with "-globals" and "coder.Constant":

>> global_values = {'coreData', coder.Constant(coreData), 'onlineData', onlineData};

>> codegen myTestnode -config cfg -globals global_values

Thanks,

Josh