Need Full Control Trajectory at Each Timestep

[Krysten Lambeth]

Hello,

I need to store grampc.rws.u, the entire control trajectory, at each time step. We created a variable called usol that we're passing throughout the optimization process, and it should show up as an extra output port in the GRAMPC Simulink block. I've attached a portion of the modified code of grampc_run_sfct below.

The new output port is not showing up, although all of the files build with no errors. Any advice is appreciated.

Thank you,
Krysten

    /*Output ports*/
    if (!ssSetNumOutputPorts(S, 8)) return;  /* number of output ports                     */
    ssSetOutputPortWidth(S, 0, Nx);          /* no. of outputs at output port 0 (xnext)    */
    ssSetOutputPortWidth(S, 1, MAX(Nu, 1));  /* no. of outputs at output port 1 (unext)    */
    ssSetOutputPortWidth(S, 2, MAX(Np, 1));  /* no. of outputs at output port 2 (pnext)    */
    ssSetOutputPortWidth(S, 3, 1);           /* no. of outputs at output port 3 (Tnext)    */
    ssSetOutputPortWidth(S, 4, 2);           /* no. of outputs at output port 4 (J)        */
    ssSetOutputPortWidth(S, 5, 1);           /* no. of outputs at output port 5 (||cfct||) */
    ssSetOutputPortWidth(S, 6, 1);           /* no. of outputs at output port 5 (||pen||)  */
    ssSetOutputPortWidth(S, 7, Nu*Ntimestep);           /* no. of outputs at output port 6 (solved 'u')   */
    ssSetOutputPortWidth(S, 8, 1);           /* no. of outputs at output port 7 (status)   */
    /*ssSetOutputPortWidth(S, 8, 1);*/           /* no. of outputs at output port 7 (iter)     */

    for (i = 0; i < 8; i++) {
        ssSetOutputPortDataType(S, i, SS_TYPERNUM);
    }
    ssSetOutputPortDataType(S, 8, SS_INT32);

[Andreas Völz]

Dear Krysten,

I think that you should change the number 8 in the if-clause with ssSetNumOutputPorts to 9 since you added one more output port. Can you please try this?

Best regards,
Andreas Völz

[Krysten Lambeth]

Thank you. I've made the necessary change, but when I update the model diagram, the new port is still not showing up.

    /*Output ports*/
    if (!ssSetNumOutputPorts(S, 9)) return;  /* number of output ports                     */
    ssSetOutputPortWidth(S, 0, Nx);          /* no. of outputs at output port 0 (xnext)    */
    ssSetOutputPortWidth(S, 1, MAX(Nu, 1));  /* no. of outputs at output port 1 (unext)    */
    ssSetOutputPortWidth(S, 2, MAX(Np, 1));  /* no. of outputs at output port 2 (pnext)    */
    ssSetOutputPortWidth(S, 3, 1);           /* no. of outputs at output port 3 (Tnext)    */
    ssSetOutputPortWidth(S, 4, 2);           /* no. of outputs at output port 4 (J)        */
    ssSetOutputPortWidth(S, 5, 1);           /* no. of outputs at output port 5 (||cfct||) */
    ssSetOutputPortWidth(S, 6, 1);           /* no. of outputs at output port 5 (||pen||)  */
    ssSetOutputPortWidth(S, 7, Nu*Ntimestep);           /* no. of outputs at output port 6 (solved 'u')   */
    ssSetOutputPortWidth(S, 8, 1);           /* no. of outputs at output port 7 (status)   */
    /*ssSetOutputPortWidth(S, 8, 1);*/           /* no. of outputs at output port 7 (iter)     */

    for (i = 0; i < 8; i++) {
        ssSetOutputPortDataType(S, i, SS_TYPERNUM);
    }
    ssSetOutputPortDataType(S, 8, SS_INT32);

[Andreas Völz]

Dear Krysten,

I am sorry, but I am not a Simulink expert. After changing the code of the S-function, I opened the Simulink model, performed "Compile the toolbox", "Compile probfct", "Init grampc" and then "Run". This lead to an error that one of the output ports is not connected and it turned out that inside the MPC-subsystem the new output port has appeared. I am not sure whether all those steps are really necessary, but it seemed that I had to run the model so that Simulink recognized that there is a new output port.

Besides, I think that there is no way of getting the value of grampc->opt->Nhor inside mdlInitializeSizes so that you have to hardcode it. This would mean that the resulting S-function can only be used with this specific example and choice of the horizon. Or did you find a solution that works with varying horizon?

Best regards,
Andreas Völz

[Krysten Lambeth]

Ok, I now see an additional output port, and it's outputting data of the correct size (Nhor elements per timestep). It's fine if it only works for this specific example and horizon length. Unfortunately, I'm just getting zeros; the control solution trajectory is not being output.

Is there another way I can access the full control solution at each time step? I had to make extensive modifications to the .c and .h scripts to pass grampc.rws.usol as an output, and a simpler method would be ideal.

[Andreas Völz]

Dear Krysten,

I don't think that you need any modifications to .c- and .h-files except for grampc_run_Sfct_Cmex.c. For my tests I added the new output port in mdlInitializeSizes just like you did. Then I modified mdlOutputs with a line

typeRNum *rws_u = (typeRNum *)ssGetOutputPortRealSignal(S, 7);

and after the call to grampc_run I added another line

MatCopy(rws_u, grampc->rws->u, grampc->opt->Nhor, grampc->param->Nu);

I connected the respective signal in Simulink to a scope and what it displayed looked roughly like what I excepted. Therefore, I did not dig deeper into it.

Best regards,
Andreas Völz

[Krysten Lambeth]

Hi Andreas,

Thank you so much! That is very helpful; I didn't realize it was such a simple change.

One last question (hopefully) - in mdlInitializeSizes, what did you declare as the size of the output port for grampc.rws.u? For some reason, while the toolbox and probfct.c compile just fine, Simulink crashes when I attempt to build the model. For Nhor = 11, I am using the following:

    ssSetOutputPortWidth(S, 8, Nu*11);     /* no. of outputs at output port 8 (usol)     */

[Andreas Völz]

Dear Krysten,

I tested with the BallOnPlate example where Nu is 1 and Nhor is 10. In mdlInitializeSizes I have

    /*Output ports*/
    if (!ssSetNumOutputPorts(S, 9)) return;         /* number of output ports                     */
    ssSetOutputPortWidth(S, 0, Nx);                 /* no. of outputs at output port 0 (xnext)    */
    ssSetOutputPortWidth(S, 1, MAX(Nu, 1));         /* no. of outputs at output port 2 (unext)    */
    ssSetOutputPortWidth(S, 2, MAX(Np, 1));         /* no. of outputs at output port 3 (pnext)    */
    ssSetOutputPortWidth(S, 3, 1);                  /* no. of outputs at output port 4 (Tnext)    */
    ssSetOutputPortWidth(S, 4, 2);                  /* no. of outputs at output port 5 (J)        */
    ssSetOutputPortWidth(S, 5, 1);                  /* no. of outputs at output port 6 (||cfct||) */
    ssSetOutputPortWidth(S, 6, 1);                  /* no. of outputs at output port 7 (||pen||)  */
    ssSetOutputPortWidth(S, 7, 10);                 /* no. of outputs at output port 8 (rws.u)  */
    ssSetOutputPortWidth(S, 8, 1);                  /* no. of outputs at output port 9 (status)   */

    for (i = 0; i < 8; i++) {
        ssSetOutputPortDataType(S, i, SS_TYPERNUM);
    }
    ssSetOutputPortDataType(S, 8, SS_INT32);

and in mdlOutputs

    /* outputs */
    typeRNum *xnext = (typeRNum *)ssGetOutputPortRealSignal(S, 0);
    typeRNum *unext = (typeRNum *)ssGetOutputPortRealSignal(S, 1);
    typeRNum *pnext = (typeRNum *)ssGetOutputPortRealSignal(S, 2);
    typeRNum *Tnext = (typeRNum *)ssGetOutputPortRealSignal(S, 3);
    typeRNum *cost = (typeRNum *)ssGetOutputPortRealSignal(S, 4);
    typeRNum *cfct = (typeRNum *)ssGetOutputPortRealSignal(S, 5);
    typeRNum *pen = (typeRNum *)ssGetOutputPortRealSignal(S, 6);
    typeRNum *rws_u = (typeRNum *)ssGetOutputPortRealSignal(S, 7);
    typeInt *status = (typeInt *)ssGetOutputPortRealSignal(S, 8);

and later

    /* Copy the solution */
    MatCopy(xnext, grampc->sol->xnext, 1, grampc->param->Nx);
    MatCopy(unext, grampc->sol->unext, 1, grampc->param->Nu);
    MatCopy(pnext, grampc->sol->pnext, 1, grampc->param->Np);
    Tnext[0] = grampc->sol->Tnext;
    MatCopy(cost, grampc->sol->J, 1, 2);
    cfct[0] = grampc->sol->cfct;
    pen[0] = grampc->sol->pen;
    MatCopy(rws_u, grampc->rws->u, grampc->opt->Nhor, grampc->param->Nu);
    status[0] = grampc->sol->status;

Did you check that your port numbers are correct and the order of the port connections matches? In your first post you had the index 7 for the rws.u-port but in your last reply you used the index 8. Better check this again carefully.

Best regards,
Andreas Völz

[Krysten Lambeth]

That worked! The issue seemed to be an unnecessary change I had previously made to another file. I'm now able to get the control solution. Thank you again kindly for your help.