Following a time varing trajectory with GRAMPC in Matlab environment

[Bin Zhong]

Hi,
Here is Bin. I am working on the GRAMPC implementation, first of all, great thanks to all the developers for providing such a convenient tool!!

I have finished the problem formulation by modifying the BallOnPlate example.
~~~void ffct(typeRNum out, ctypeRNum t, ctypeRNum x, ctypeRNum u, ctypeRNum p, typeUSERPARAM userparam)
{
ctypeRNum pSys = (ctypeRNum*)userparam;
ctypeRNum A = pSys[0];
ctypeRNum B = pSys[1];
ctypeRNum J = pSys[2];
ctypeRNum tau_g = pSys[3];

out[0] = x[1];
out[1] = (u[0] - A * tanh(10 * x[1]) - B * x[1] - tau_g * sin(x[0])) / J;

}
void lfct(typeRNum out, ctypeRNum t, ctypeRNum x, ctypeRNum u, ctypeRNum p, ctypeRNum xdes, ctypeRNum udes, typeUSERPARAM userparam)
{
ctypeRNum pCost = (ctypeRNum*)userparam;
ctypeRNum w_theta = pCost[4];
ctypeRNum w_tau = pCost[5];

out[0] = w_theta * POW2((x[0] - xdes[0])) + w_tau * POW2((u[0] - udes[0]));

~~~
With the constant setpoint x_des, the model seemed to work fine with similar results to other studies. The probfct.c file has also been enclosed. The next step, I want to evaluate the following performance with a time varying trajectory, e.g., a sinusoidal trajectory. I have reviewed the discussion posted before (https://sourceforge.net/p/grampc/discussion/general/thread/cd2e89b28f/), and I rewrited the ffct and vfct function in probfct.c, and changed the involved xdes[0] to the time-varying reference trajectory x_ref as below:
`void lfct(typeRNum out, ctypeRNum t, ctypeRNum x, ctypeRNum u, ctypeRNum p, ctypeRNum xdes, ctypeRNum udes, typeUSERPARAM userparam)
{
ctypeRNum pCost = (ctypeRNum*)userparam;
ctypeRNum w_theta = pCost[4];
ctypeRNum w_tau = pCost[5];

ctypeRNum t0 = pCost[10];
ctypeRNum x_ref = 1 * sin(2 * 3.14 * (t0 + t));

out[0] = w_theta * POW2((x[0] - x_ref)) + w_tau * POW2((u[0] - udes[0]));

}
void dldx(typeRNum out, ctypeRNum t, ctypeRNum x, ctypeRNum u, ctypeRNum p, ctypeRNum xdes, ctypeRNum udes, typeUSERPARAM userparam)
{
ctypeRNum pCost = (ctypeRNum*)userparam;
ctypeRNum w_theta = pCost[4];

ctypeRNum t0 = pCost[10];
ctypeRNum x_ref = 1 * sin(2 * 3.14 * (t0 + t));

out[0] = 2 * w_theta * (x[0] - x_ref);

}
`
I realized that I should update the globle time somewhere in the MPC loop in Matlab, but I don't know how, hope you can give me some advice.

Thanks,
Bin

[Andreas Völz]

Dear Bin Zhong,

you need to set the respective entry of userparam to the current time before the call to grampc_run, that is like

grampc.userparam(11) = vec.t(i);
[grampc,vec.CPUtime(i)] = CmexFiles.grampc_run_Cmex(grampc);

See also https://sourceforge.net/p/grampc/discussion/general/thread/cd2e89b28f/#cf73/f55d where the same is done in C code.

Best regards,
Andreas Völz

[Bin Zhong]

Andreas Völz,

Great thanks for your timely reply ! Thanks for your suggestion.

I have tried to modified the code as:

    % run MPC and save results
    grampc.userparam(11) = vec.t(i);      %update the current time
    [grampc,vec.CPUtime(i)] = CmexFiles.grampc_run_Cmex(grampc);
    vec = grampc_update_struct_sol(grampc, vec, i);

But the result shows that the state still convergent to the constant setpoint, i.e., x_des[1.2, 0], while not track the trajectory: x_ref = 1 * sin(2 * 3.14 * (t0 + t));

I have also tried to inplement the sin trajectory directly in the startMPC.m as below:

    % run MPC and save results
    grampc.userparam(11) = sin(6.28 * vec.t(i));  
    [grampc,vec.CPUtime(i)] = CmexFiles.grampc_run_Cmex(grampc);
    vec = grampc_update_struct_sol(grampc, vec, i);

and revised the corresponding procfct.c file, also not working.

I am not sure which step or setup lead to this. I have also attached the complete project (under the folder: GRAMPC_v2.2\examples\BallOnPlate_modified). I will try to address this at this moment. I would appreciate if you can kinly help to check the code. If you are not available, just leave this alone :).

Have a nice weekend!

Best regard,
Bin

[Bin Zhong]

Andreas Völz,

Great thanks for your timely reply ! Thanks for your suggestion.

I have tried to modified the code as:

    % run MPC and save results
    grampc.userparam(11) = vec.t(i);      %update the current time
    [grampc,vec.CPUtime(i)] = CmexFiles.grampc_run_Cmex(grampc);
    vec = grampc_update_struct_sol(grampc, vec, i);

But the result shows that the state still convergent to the constant setpoint, i.e., x_des[1.2, 0], while not track the trajectory: x_ref = 1 * sin(2 * 3.14 * (t0 + t));

I have also tried to inplement the sin trajectory directly in the startMPC.m as below:

    % run MPC and save results
    grampc.userparam(11) = sin(6.28 * vec.t(i));  
    [grampc,vec.CPUtime(i)] = CmexFiles.grampc_run_Cmex(grampc);
    vec = grampc_update_struct_sol(grampc, vec, i);

and revised the corresponding procfct.c file, also not working.

I am not sure which step or setup lead to this. I have also attached the complete project (under the folder: GRAMPC_v2.2\examples\BallOnPlate_modified). I will try to address this at this moment. I would appreciate if you can kinly help to check the code. If you are not available, just leave this alone :).

Have a nice weekend!

Best regard,
Bin

[Andreas Völz]

Dear Bin Zhong,

I have tested your code and think the main problem was that your trajectory is not feasible for the system. In the attached code I have modified the desired trajectory, the initial state and removed the wrong lines in startMPC. With these changes, the first state follows the trajectory as expected. To further improve the result, the desired control value should be adapted in such a way that exact tracking has zero cost.

Best regards,
Andreas Völz

[Bin Zhong]

Dear Andreas Völz,

You are life saver ！Great thanks for your kind help, this question bothered me for the past two days.

Best regards,
Bin