time-varying reference state trajectory

[WenChao Xu]

Hello everyone!

  I read the documentation of GRAMPC and some references using GRAMPC, and some of them mentioned that GRAMPC uses fixed reference trajectories xdes and udes by default. I would like to ask how to implement a reference trajectory that tracks single or multiple states?

Thanks.
Best regards,
Xu.

[WenChao Xu]

I read this discussion https://sourceforge.net/p/grampc/discussion/general/thread/ab9aafc9a5/ ; But when I try to modify BallOnPlate, the result does not seem to be correct.

[Andreas Völz]

Dear WenChao Xu,

the parameters xdes and udes are just offered as convenience for the frequent case of setpoints that are constant over the prediction horizon. With help of userparam, arbitrary complex scenarios can be realized on top of GRAMPC. Actually, this is described quite well in the thread that you have linked. Therefore, I would ask what have you tried and what were the results?

Important to note is that the time argument in the cost functions and constraints is the internal prediction time going from 0 to Thor, whereas it is the global time going from t0 to t0+Thor in the system dynamics functions. This means that for trajectory tracking, you often have to pass t0 as additional userparam to the cost function to determine with t0+t the corresponding time-varying setpoint.

Best regards,
Andreas Völz

[WenChao Xu]

Dear Andreas Völz

Thank you for your timely reply. I modified lfct and vfct in BALL_ON_PLATE, and changed the involved xdes[0] to the time-varying reference trajectory xref. At the same time, in order to only allow x[0] to achieve reference trajectory tracking, I set the corresponding weight value of x[1] to 0. The modified code is as follows:

void lfct(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, ctypeRNum *xdes, ctypeRNum *udes, typeUSERPARAM *userparam)
{
    ctypeRNum* param = (ctypeRNum*)userparam;
    typeRNum t0 = param[9];
    typeRNum* coeff = param + 10;

    typeRNum xref = coeff[0] * sin(t0 + t) + coeff[1] * sin(t0 + 2*t) + coeff[2];

    out[0] = (param[0] * POW2(x[0] - xref)
            + param[1] * POW2(x[1] - xdes[1])
            + param[2] * POW2(u[0] - udes[0])) / 2;
}
/** Gradient dl/dx **/
void dldx(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, ctypeRNum *xdes, ctypeRNum *udes, typeUSERPARAM *userparam)
{
    ctypeRNum* param = (ctypeRNum*)userparam;
    typeRNum t0 = param[9];
    typeRNum* coeff = param + 10;

    typeRNum xref = coeff[0] * sin(t0 + t) + coeff[1] * sin(t0 + 2*t) + coeff[2];

    out[0] = param[0] * (x[0] - xref);
    out[1] = param[1] * (x[1] - xdes[1]);
}
/** Gradient dl/du **/
void dldu(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, ctypeRNum *xdes, ctypeRNum *udes, typeUSERPARAM *userparam)
{
    ctypeRNum* param = (ctypeRNum*)userparam;

    out[0] = param[2] * (u[0] - udes[0]);
}

void Vfct(typeRNum *out, ctypeRNum T, ctypeRNum *x, ctypeRNum *p, ctypeRNum *xdes, typeUSERPARAM *userparam)
{
    ctypeRNum* param = (ctypeRNum*)userparam;
    typeRNum t0 = param[9];
    typeRNum* coeff = param + 10;

    typeRNum xref = coeff[0] * sin(t0 + T) + coeff[1] * sin(t0 + 2*T) + coeff[2];

    out[0] = (param[3] * POW2(x[0] - xref)
            + param[4] * POW2(x[1] - xdes[1])) / 2;
}
/** Gradient dV/dx **/
void dVdx(typeRNum *out, ctypeRNum T, ctypeRNum *x, ctypeRNum *p, ctypeRNum *xdes, typeUSERPARAM *userparam)
{
    ctypeRNum* param = (ctypeRNum*)userparam;
    typeRNum t0 = param[9];
    typeRNum* coeff = param + 10;

    typeRNum xref = coeff[0] * sin(t0 + T) + coeff[1] * sin(t0 + 2*T) + coeff[2];

    out[0] = param[3] * (x[0] - xref);
    out[1] = param[4] * (x[1] - xdes[1]);
}

    ctypeRNum x0[NX] = { (typeRNum)0.1,(typeRNum)0.01 };
    ctypeRNum xdes[NX] = { (typeRNum)-0.2, 0.0 };

    /********* userparam *********/
    typeRNum pCost[13] = { 100, 0, 180,          
                           100, 0,               
                           -0.2, 0.2, -0.1, 0.1, /*Inequality constraints*/
                           t, 1.0, 0.7, 1.5};    /*time-varying reference trajectory*/
    typeUSERPARAM *userparam = pCost;

Then after I run the code, the data in xvec.txt in res looks a bit incorrect. x[0] does not seem to track the reference trajectory, and x[1] still converges to 0. Is it because of the terminal cost item in the cost function? When I used MPC before, I didn't use this item much.

Best regards,

Xu.

[WenChao Xu]

Dear Andreas Völz

Thank you for your timely reply. I modified lfct and vfct in BALL_ON_PLATE, and changed the involved xdes[0] to the time-varying reference trajectory xref. At the same time, in order to only allow x[0] to achieve reference trajectory tracking, I set the corresponding weight value of x[1] to 0. The modified code is as follows:

void lfct(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, ctypeRNum *xdes, ctypeRNum *udes, typeUSERPARAM *userparam)
{
    ctypeRNum* param = (ctypeRNum*)userparam;
    typeRNum t0 = param[9];
    typeRNum* coeff = param + 10;

    typeRNum xref = coeff[0] * sin(t0 + t) + coeff[1] * sin(t0 + 2*t) + coeff[2];

    out[0] = (param[0] * POW2(x[0] - xref)
            + param[1] * POW2(x[1] - xdes[1])
            + param[2] * POW2(u[0] - udes[0])) / 2;
}
/** Gradient dl/dx **/
void dldx(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, ctypeRNum *xdes, ctypeRNum *udes, typeUSERPARAM *userparam)
{
    ctypeRNum* param = (ctypeRNum*)userparam;
    typeRNum t0 = param[9];
    typeRNum* coeff = param + 10;

    typeRNum xref = coeff[0] * sin(t0 + t) + coeff[1] * sin(t0 + 2*t) + coeff[2];

    out[0] = param[0] * (x[0] - xref);
    out[1] = param[1] * (x[1] - xdes[1]);
}
/** Gradient dl/du **/
void dldu(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, ctypeRNum *xdes, ctypeRNum *udes, typeUSERPARAM *userparam)
{
    ctypeRNum* param = (ctypeRNum*)userparam;

    out[0] = param[2] * (u[0] - udes[0]);
}

void Vfct(typeRNum *out, ctypeRNum T, ctypeRNum *x, ctypeRNum *p, ctypeRNum *xdes, typeUSERPARAM *userparam)
{
    ctypeRNum* param = (ctypeRNum*)userparam;
    typeRNum t0 = param[9];
    typeRNum* coeff = param + 10;

    typeRNum xref = coeff[0] * sin(t0 + T) + coeff[1] * sin(t0 + 2*T) + coeff[2];

    out[0] = (param[3] * POW2(x[0] - xref)
            + param[4] * POW2(x[1] - xdes[1])) / 2;
}
/** Gradient dV/dx **/
void dVdx(typeRNum *out, ctypeRNum T, ctypeRNum *x, ctypeRNum *p, ctypeRNum *xdes, typeUSERPARAM *userparam)
{
    ctypeRNum* param = (ctypeRNum*)userparam;
    typeRNum t0 = param[9];
    typeRNum* coeff = param + 10;

    typeRNum xref = coeff[0] * sin(t0 + T) + coeff[1] * sin(t0 + 2*T) + coeff[2];

    out[0] = param[3] * (x[0] - xref);
    out[1] = param[4] * (x[1] - xdes[1]);
}

    ctypeRNum x0[NX] = { (typeRNum)0.1,(typeRNum)0.01 };
    ctypeRNum xdes[NX] = { (typeRNum)-0.2, 0.0 };

    /********* userparam *********/
    typeRNum pCost[13] = { 100, 0, 180,          
                           100, 0,               
                           -0.2, 0.2, -0.1, 0.1, /*Inequality constraints*/
                           t, 1.0, 0.7, 1.5};    /*time-varying reference trajectory*/
    typeUSERPARAM *userparam = pCost;

Then after I run the code, the data in xvec.txt in res looks a bit incorrect. x[0] does not seem to track the reference trajectory, and x[1] still converges to 0. Is it because of the terminal cost item in the cost function? When I used MPC before, I didn't use this item much.

Best regards,

Xu.

[Andreas Völz]

Dear WenChao Xu,

your basic approach seems correct to me. However, I guess that

typeRNum xref = coeff[0] * sin(t0 + t) + coeff[1] * sin(t0 + 2*t) + coeff[2];

should be changed into

typeRNum xref = coeff[0] * sin(t0 + t) + coeff[1] * sin(2 * (t0 + t)) + coeff[2];

Furthermore, you need to update the global time in each MPC step, that is, the MPC loop should be modified as

        userparam[9] = t;
        grampc_run(grampc);

Please try this and report on the results.

Best regards,
Andreas Völz

[WenChao Xu]

Dear Andreas Völz

Thank you very much for your reply. I modified it according to your comment. One difference is that,userparam[9] = t; ----> pcost[9]=t

    /********* userparam *********/
    typeRNum pCost[13] = { 100, 0, 180,          
                           100, 0,               
                           -0.2, 0.2, -0.1, 0.1, /*Inequality constraints*/
                           t, 0.1, 0.05, 0.1};    /*time-varying reference trajectory*/
    typeUSERPARAM *userparam = pCost;

    for (iMPC = 0; iMPC <= MaxSimIter; iMPC++) {
        /* run grampc */
        tic = clock();
        pCost[9] = t; //difference, not 'userparam[9] = t;'
        grampc_run(grampc);

The result of the operation is shown in figure. The generation of reference x1 involved in it is also in the linked file.

Best regards,
Xu.

[Andreas Völz]

Dear WenChao Xu,

looks like it is basically working. The next step would be to check the influence of different parameters (weighting factors, number of iterations, ...) on the tracking performance. Furthermore, I am not sure if your reference trajectory is feasible with respect to the system dynamics, that is, whether perfect tracking would be possible at all.

Best regards,
Andreas Völz

[WenChao Xu]

Dear Andreas Völz

Thank you very much for your help. This reference track is just to verify how to track the time-varying reference track. I am going to implement a complete MPC these days.

Thank you again! Your response was very timely and resolved my doubts, thank you very much!

Best regards,

Xu.

[Andreas Völz]

Dear WenChao Xu,

thanks for the positive feedback. Some more complex examples (e.g. trajectory tracking, C++ interface, ...) are still on the "to do list" for the next update of GRAMPC.

Best regards,
Andreas Völz

[WenChao Xu]

Dear Andreas Völz,

Hello! Does it only need to write the following code during the running of MPC to implement the penalty term for the control input signal rate:

grampc_setparam_real_vector(grampc, "udes", grampc->sol->unext);

the penalty term for the control input signal rate is: ||Δu||, where Δu=uk - uk-1.

lfct function:

    out[0] = (param[0] * POW2(x[0] - xref)
                         + param[1] * POW2(x[1] - xdes[1])
                         + param[2] * POW2(u[0] - udes[0])) / 2;

Best regards,
WenChao Xu.

[Andreas Völz]

Dear WenChao Xu,

if you set unext as udes, this is some kind of regularization but not a weighting of the control input rate. The typical approach would be to use the time derivative of your true input as virtual input in GRAMPC and add an integrator to the model to obtain the true input as integral of the virtual input. Then you can use umax and umin to limit the control input rate. Note however, that the constraints on the true input must then be realized as state constraint via hfct.

Best regards,
Andreas Völz

[WenChao Xu]

Dear Andreas Völz,
Your reply is so fast, thank you very much!

if you set unext as udes, this is some kind of regularization but not a weighting of the control input rate.

In my understanding, shouldn't regularization and penalty terms have the same meaning? My purpose is to add a penalty term to the control input change in the cost function, so that the control variable does not change drastically in a short period of time. According to my understanding it is feasible to set unext to udes. Is not my understanding of the problem?😔

Best regards,
WenChao Xu.

[Andreas Völz]

Dear WenChao Xu,

if you set udes to unext, the result will be a damping of the control updates. The higher the weighting, the closer the next "unext" will be to the previous "unext". However, my point was that this "unext" is the same along the whole prediction horizon, that is, for all t \in [0, Thor], the difference of u(t) to unext is weighted. This is different from weighting \dot u(t) or the discrete approximation u(t) - u(t + \delta t). Both is feasible of course and it depends on what you actually want to achieve.

Best regards,
Andreas Völz

[WenChao Xu]

Dear Andreas Völz,

Ok, I tried to implement this, thanks a lot!😊

Best regards,
WenChao Xu.

[WenChao Xu]

Dear Andreas Völz,

Hello Andreas Völz. Hahaha, I'm here to ask a question again, and I'm a little embarrassed.😂😂😂
I tried the method you mentioned above, but I ran into some problems. I modified code based on BallOnPlate:
Letu_real = x3; u_virtual=u1; And take the upper and lower limits of the real control as the constraints of the new state, and the upper and lower limits of the virtual control, i.e. , the constraints of the control input rate are: [-0.0524,0.0524]. This range needs to be modified according to requirements, here is an example.

void ffct(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, typeUSERPARAM *userparam)
{
    out[0] = (typeRNum)(-0.04)*x[2] + x[1];
    out[1] = (typeRNum)(-7.01)*x[2];
    out[2] = (typeRNum)u[0];
}

void lfct(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, ctypeRNum *xdes, ctypeRNum *udes, typeUSERPARAM *userparam)
{
    ctypeRNum* param = (ctypeRNum*)userparam;
    typeRNum t0 = param[13];

    typeRNum xref = 0.1 * sin(t0 + t) + 0.05 * sin(2 * (t0 + t));

    out[0] = (param[0] * POW2(x[0] - xref)
            + param[1] * POW2(x[1] - xdes[1])
            + param[2] * POW2(x[2] - xdes[2])
            + param[3] * POW2(u[0] - udes[0])) / 2;
}

void Vfct(typeRNum *out, ctypeRNum T, ctypeRNum *x, ctypeRNum *p, ctypeRNum *xdes, typeUSERPARAM *userparam)
{
    ctypeRNum* param = (ctypeRNum*)userparam;
    typeRNum t0 = param[13];

    typeRNum xref = 0.1 * sin(t0 + T) + 0.05 * sin(2 * (t0 + T));

    out[0] = (param[4] * POW2(x[0] - xref)
            + param[5] * POW2(x[1] - xdes[1])
            + param[6] * POW2(x[2] - xdes[2])) / 2;
}

void hfct(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, typeUSERPARAM *userparam)
{
    ctypeRNum* param = (ctypeRNum*)userparam;

    out[0] =  param[7] - x[0];
    out[1] = -param[8] + x[0];
    out[2] =  param[9] - x[1];
    out[3] = -param[10] + x[1];
    out[4] =  param[11] - x[2];
    out[5] = -param[12] + x[2];
}

    ctypeRNum x0[NX] = { (typeRNum)0.0,(typeRNum)0.0,(typeRNum)0.0 };
    ctypeRNum xdes[NX] = { (typeRNum)-0.2, 0.0, 0.0 };

    ctypeRNum u0[NU] = { 0.0 };
    ctypeRNum udes[NU] = { 0.0 };
    ctypeRNum umax[NU] = { (typeRNum)0.0524 };
    ctypeRNum umin[NU] = { (typeRNum)-0.0524 };

    typeRNum pCost[14] = { 100, 0, 10, 10,       
                           100, 0, 10,           
                           -0.2, 0.2, -0.1, 0.1, -0.0524, 0.0524,   /*Inequality constraints*/
                           t                                        /*time-varying reference trajectory*/
                           };    
    typeUSERPARAM *userparam = pCost;

        /* reference integration of the system via heun scheme since grampc->sol->xnext is only an interpolated value */
        ffct(rwsReferenceIntegration, t, grampc->param->x0, grampc->sol->unext, grampc->sol->pnext, grampc->userparam);
        for (i = 0; i < NX; i++) {
            grampc->sol->xnext[i] = grampc->param->x0[i] + dt * rwsReferenceIntegration[i];
        }
        ffct(rwsReferenceIntegration + NX, t + dt, grampc->sol->xnext, grampc->sol->unext, grampc->sol->pnext, grampc->userparam);
        for (i = 0; i < NX; i++) {
            grampc->sol->xnext[i] = grampc->param->x0[i] + dt * (rwsReferenceIntegration[i] + rwsReferenceIntegration[i + NX]) / 2;
        }

The output result after running feels unreasonable, as shown in the figure below:

State Trajectory and Reference Trajectory;
Control input rate;
cost curve.

Best regards,
WenChao Xu.

[Andreas Völz]

Dear WenChao Xu,

thanks for sharing the code and the plots. I guess that you also implemented dhdx_vec and not only hfct although you didn't show it. The problem may be either an implementation error (although I saw no obvious one) or a matter of tuning, which is clearly more difficult with the additional integrator. From the plots it seems that it may be impossible for the system to follow the trajectory with these input rate limits. Since the system is too slow, it lags behind the reference, which then leads to instability of the MPC. Maybe you try it with a slower reference trajectory or larger bounds on the rate?

Otherwise, I could try to implement a minimal working example, but it could take some time, since I have a lot of things to do currently.

Best regards,
Andreas Völz

[WenChao Xu]

Dear Andreas Völz,

Thank you for your reply!
Yes, I also implemented dhdx_vec.I'm going to modify the code now.

Otherwise, I could try to implement a minimal working example, but it could take some time, since I have a lot of things to do currently.

It doesn't matter, you just do your own thing, don't spend time writing examples. I hope you will become an academic leader soon, thank you 💖

Best regards,
WenChao Xu.

[Andreas Völz]

Dear WenChao Xu,

another idea would be to test the rate limits first for the simpler task of setpoint stabilization. That is, replace the reference trajectory with constant desired values and see whether you can tune the MPC for exponentially decreasing cost.

Best regards,
Andreas Völz

[WenChao Xu]

Dear Andreas Völz,

I tested it, and did the same test with another model, and it seems the result is still not good. I'm checking to see if there are any errors.

Best regards
WenChao Xu.