implementin NMPC in ROS using code generation

Question

Mikael_P 2023-5-11

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1961399-implementin-nmpc-in-ros-using-code-generation

回答： Josh Chen 2023-5-12

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!

4 个评论
显示 2更早的评论隐藏 2更早的评论

Mikael_P 2023-5-11

在 MATLAB Online 中打开

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 2023-5-11

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 2023-5-12

编辑：Mikael_P 2023-5-12

在 MATLAB Online 中打开

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 2023-5-12

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1961399-implementin-nmpc-in-ros-using-code-generation#answer_1233969

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

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

implementin NMPC in ROS using code generation

4 个评论
显示 2更早的评论隐藏 2更早的评论

采纳的回答

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

更多回答（0 个）

另请参阅

类别

标签

产品

版本

Community Treasure Hunt

implementin NMPC in ROS using code generation

4 个评论 显示 2更早的评论隐藏 2更早的评论

采纳的回答

0 个评论 显示 -2更早的评论隐藏 -2更早的评论

更多回答（0 个）

另请参阅

类别

标签

产品

版本

Community Treasure Hunt

4 个评论
显示 2更早的评论隐藏 2更早的评论

0 个评论
显示 -2更早的评论隐藏 -2更早的评论