I have a time-varying trajectory that is piecewise continuous (based on a finite state machine) and would be difficult to incorporate into probfct.c. My understanding is that in the image attached, GRAMPC will consider xdes to be held constant over the prediction horizon. In other words, only the first point in the trajectory will be passed to probfct.c. Is that correct?
thank you for your interest in GRAMPC. You are right that xdes is assumed constant over the prediction horizon, it is just a convenience parameter for the typical setpoint stabilization tasks. Trajectory tracking should be implemented via the userparam field depending on the problem-specific representation (e.g. discrete sampling points along the horizon or a polynomial trajectory). To get started, see for example the past questions:
Thank you so much for the clarification. Follow-up question - was this also the case for the older version of GRAMPC (as shown in the attached image)? That is, would the old version also hold xdes constant for the prediction horizon?
yes, xdes and udes have always been parameters of size Nx and Nu that were simply passed to the cost functions and their derivatives. In order to take time-varying references into account, you always had to work with userparam (or pCost in v1.0 and v1.1). However, we generally see this as advantage since one can realize arbitrary complex usecases with the userparam field.
Do you need help with implementing your usecase via userparam?
Best regards,
Andreas Völz
If you would like to refer to this comment somewhere else in this project, copy and paste the following link:
I did found this example helpful (https://sourceforge.net/p/grampc/discussion/general/thread/21114e01aa/) . I created a vector with a length equal to the prediction horizon length (10 steps, or 0.1s, in my case). This vector is added to grampc.userparam and updated at each step in the Trajectory Generator subsystem. Then, the cost function uses the vector xd (instead of xdes), where xd's value depends on userparam and time t. Relevant code is below.
I've included an image which shows how I'm passing the desired trajectory. The "current" xd (named xdnext in the image) is still passed to the xdes port, although this shouldn't be doing anything.
This seems to be working, even if the tracking error is higher than I would like it to be. Does this all look correct?
this definitely goes into the right direction. You should take care that if you want to cover a horizon Thor=1.0s with sampling points every 0.1s then you need Nhor=11 because the sampling time along the horizon is Thor/(Nhor-1).
You should also take care that Matlab starts counting from 1 whereas C code starts counting from 0. The element pSys[13] is thus the 14th element in the Matlab vector. Your first step is 0 for t=0 and therefore your first xd is selected from pSys[13+2*0+0] which also evaluates to pSys[13]. Probably not what you want to have.
Finally, do you really have a dense Q matrix for the weighting? We nearly always use diagonal weighting matrices and then you can replace the double loop for Q'x by a single loop that multiplies the diagonal entry of Q with the respective deviation. In fact we usually use only a vector with Nx elements for Q.
Best regards,
Andreas Völz
If you would like to refer to this comment somewhere else in this project, copy and paste the following link:
You're right; I needed to add a +1 to the index for pSys. As for Q, I've now simplified it as a vector instead of a sparse matrix.
Perhaps this is a silly question, but since I'm not overly familiar with C, is there any way I can verify that step and xd are being correctly defined (i.e., print them out)? I ask because I initially received compilation errors regarding the index for pSys not being an integer because it depends on the quotient of two doubles, t/d_t. I played around with it and was able to get it to run using the definition of step in my earlier post, but I don't really know what I changed in probfct.c that caused it to successfully compile. The tracking performance, optimal inputs, and cost are nearly identical to the version that incorrectly held xd constant over Nhor, which makes me skeptical whether xd is really changing with each time step.
If you would like to refer to this comment somewhere else in this project, copy and paste the following link:
the code pSys[t/d_t] does not compile because t/d_t is of floating point type but array access requires integer types. With int step = t/d_t; you perform an implicit type cast from floating point to integer. Alternatively you could use pSys[(int)(t/d_t)] with an explicit type cast.
In principle it is possible to use a debugger to go through the code step by step. While this is easy if you run GRAMPC as pure C code, it is more involved if you start it from Matlab and probably even more complicated if you run it within Simulink and requires to attach the debugger to an externally running process. My own experience is quite limited in this regard.
However, you can also add printf-statements directly in the probfct. We have in include/grampc_mess.h a macro myPrint with two arguments that either maps to printf or to mexPrintf if you run your MPC from Matlab. You could use this within your problem functions. My recommendation would then be to set MaxGradIter=1, MaxMultIter=1 and Tsim=dt so that you only run a single gradient iteration. Otherwise, you probably get too many messages printed.
Another option is to solve only the OCP in the first MPC step and look at the resulting trajectories in grampc.rws, that is, the predicted trajectories. If you run many iterations, the predicted trajectories should already follow the reference trajectory. If the desired values were constant, this should also be visible in the optimal predicted trajectories.
Best regards,
Andreas Völz
If you would like to refer to this comment somewhere else in this project, copy and paste the following link:
Thank you again for your detailed advice, and I apologize for the continued questions. I've tried both approaches. If I solve the OCP in startMPC.m but stop the code before it moves to the next step, I'm able to view grampc.rws.x. Unfortunately, even with MaxMultIter = 1, MaxGradIter = 1, and Tsim = dt, these are still suspiciously similar.
I've attempted to use myPrint and printf in the problem function. While both of these print functions work if I include them in grampc_run_Cmex.c and grampc_run.c, I can't get anything in lfct or dldx (within probfct.c) to print. I consulted a colleague who's well-versed in c, but we weren't able to figure it out.
This may seem like nitpicking, but I'm building another probfct.c that uses xd in the system dynamics ffct, and it's critical that those are accurate or it could produce instability. So I'm debugging using a simpler example.
voiddldx(typeRNum*out,ctypeRNumt,ctypeRNum*x,ctypeRNum*u,ctypeRNum*p,ctypeRNum*xdes,ctypeRNum*udes,typeUSERPARAM*userparam){ctypeRNum*pSys=(ctypeRNum*)userparam;//AssumesthatQissymmetricinti;doubleQ[2]={pSys[10],pSys[11]};doubledx[2],xd[2];printf("%s",("We're now in dl/dx"));myPrint("%s",("We're now in dl/dx"));doubled_t=pSys[13];intstep=t/d_t;//currentstep(from0toN,whereN=#stepsinpredictionhorizon)for(i=0;i<Lx;i++){xd[i]=pSys[13+2*step+i+1];printf("%s",("We're now in xd"));myPrint("%s",("We're now in xd"));}for(i=0;i<Lx;i++)dx[i]=x[i]-xd[i];//2*Q'*xfor(i=0;i<Lx;i++){out[i]=2*Q[i]*dx[i];}}
If you would like to refer to this comment somewhere else in this project, copy and paste the following link:
Yes, it compiles. When I run the startMPC MATLAB file, it prints the messages contained in grampc_run.c and grampc_run_Cmex.c after printing the GRAMPC parameters. Each message is displayed twice when I run one iteration.
Your should add a line break to all your printf statements, so printf("%s\n",("We're now in dl/dx")). Currently you get everything in one line but I also saw "evaluateCost" which maybe your output in the lfct.
If you would like to refer to this comment somewhere else in this project, copy and paste the following link:
Thanks! Here is my updated output with line breaks. It's not printing anything in lfct within probfct.c, though. I have the print command coded at the top of lfct as follows:
voidlfct(typeRNum*out,ctypeRNumt,ctypeRNum*x,ctypeRNum*u,ctypeRNum*p,ctypeRNum*xdes,ctypeRNum*udes,typeUSERPARAM*userparam){printf("%s\n",("We're now in lfct"));
If I can get this to work, I can change what's contained in printf to show xd for that iteration. I'm concerned there's some issue with xd because if I ask my model to use the following very simple feedback control and just optimize alpha, it not only becomes unstable, but it gives radically different results each time I run it. I've tried tightening the error tolerances to no avail. The same feedback control law (using the initial guess for alpha) works in simulation without GRAMPC.
voidffct(typeRNum*out,ctypeRNumt,ctypeRNum*x,ctypeRNum*u,ctypeRNum*p,typeUSERPARAM*userparam){ctypeRNum*pSys=(ctypeRNum*)userparam;inti;/* Desired states */doublexd[2];doublealpha=p[0],d_t=pSys[12];intstep=(int)(t/d_t);//currentstepfor(i=0;i<2;i++)xd[i]=pSys[12+2*step+i+1];doublee=xd[0]-x[0];/* State-space dynamics function */doubleq=x[0]+pSys[2];doubleTkp=pSys[4]*(q-pSys[3])+pSys[5]*x[1]+pSys[6]*exp(pSys[7]*q)-pSys[8]*exp(pSys[9]*q);doubleTm=alpha*e;out[0]=x[1];out[1]=pSys[0]*(Tm-Tkp)+pSys[1]*cos(q);}
If you would like to refer to this comment somewhere else in this project, copy and paste the following link:
it is quite difficult to help here with always only a part of the code available. If you get different results with each run, the reason is typically that you read from the wrong memory locations. This can happen in C, for example when variables are not initialized properly or when you read with pointers (or arrays) beyond your allocated memory. One option would be that you reduce your code to a minimal working example that still shows the problematic behaviour or I can try to create a minimal example in the next days. Since we have already done such things multiple times, I am pretty sure that the error is somewhere in your code or in how you use it from Matlab/Simulink.
Best regards,
Andreas Völz
If you would like to refer to this comment somewhere else in this project, copy and paste the following link:
Ok, I've simplified it to a double integrator where the two gains in a PD control law need to be optimized. The initial guesses should stabilize the system, but I'm getting the same problem as before. I re-downloaded GRAMPVC v2.2 and built this version off of the template files to ensure no changes had been made to the src, include, or matlab folders that could affect the memory. If you are able to identify where the memory issue is occurring, I will be very grateful.
thanks for sharing the minimal example. Using GRAMPC to optimize over feedback gains instead of control inputs is a quite interesting usecase that we probably have not tested so far. Our initial motivation to add the parameter optimization was rather to enable moving horizon estimation.
It took me a while to debug your problem, but I think I found at least one main issue. GRAMPC passes the global time t0+t (running from t0 to t0+Thor) to ffct, dfdx_vec, dfdu_vec, dfdp_vec while it passes the local time t (running from 0 to Thor) to all other problem functions. Therefore, your dynamics computed step indices that were far beyond the userparam array. One solution would be to also pass in t0 via userparam and subtract it. A quicker solution is to comment the line
in startMPC.m so that the global time is not updated. I guess in Simulink this would correspond to passing the constant input 0 to the tk ports of the MPC subsystem and the sysfct sfunction.
With these changes I was at least able to run the example without producing nan or inf values. Maybe there are still other errors or it could mainly be a matter of tuning, but I hope that you can continue with this.
Best regards,
Andreas Völz
If you would like to refer to this comment somewhere else in this project, copy and paste the following link:
That worked! I saved t0 as a parameter per your suggestion and just subtracted it in ffct, dfdp, etc. Even in the more complex script, this solution works perfectly.
Best,
Krysten
If you would like to refer to this comment somewhere else in this project, copy and paste the following link:
See attached for my trajectory tracking MPC. It's framed as a parameter optimization problem in order to test out trajectory tracking by driving the states with a PD feedback controller. The parameter optimization part itself doesn't appear to work, actually, as it never updates the initial guess. I didn't troubleshoot this aspect since it wasn't my end goal, but I hope that the trajectory aspect is helpful!
I have a time-varying trajectory that is piecewise continuous (based on a finite state machine) and would be difficult to incorporate into probfct.c. My understanding is that in the image attached, GRAMPC will consider xdes to be held constant over the prediction horizon. In other words, only the first point in the trajectory will be passed to probfct.c. Is that correct?
Dear Krysten Lambeth,
thank you for your interest in GRAMPC. You are right that xdes is assumed constant over the prediction horizon, it is just a convenience parameter for the typical setpoint stabilization tasks. Trajectory tracking should be implemented via the userparam field depending on the problem-specific representation (e.g. discrete sampling points along the horizon or a polynomial trajectory). To get started, see for example the past questions:
https://sourceforge.net/p/grampc/discussion/general/thread/7add8e634f/
https://sourceforge.net/p/grampc/discussion/general/thread/21114e01aa/
https://sourceforge.net/p/grampc/discussion/general/thread/ab9aafc9a5/
https://sourceforge.net/p/grampc/discussion/general/thread/cd2e89b28f/
https://sourceforge.net/p/grampc/discussion/general/thread/a341eb7a10/
If you need more help, please ask. We also plan to add a trajectory-tracking example to the next version of GRAMPC.
Best regards,
Andreas Völz
Hi Andreas,
Thank you so much for the clarification. Follow-up question - was this also the case for the older version of GRAMPC (as shown in the attached image)? That is, would the old version also hold xdes constant for the prediction horizon?
Dear Krysten Lambeth,
yes, xdes and udes have always been parameters of size Nx and Nu that were simply passed to the cost functions and their derivatives. In order to take time-varying references into account, you always had to work with userparam (or pCost in v1.0 and v1.1). However, we generally see this as advantage since one can realize arbitrary complex usecases with the userparam field.
Do you need help with implementing your usecase via userparam?
Best regards,
Andreas Völz
Hi Andreas,
I did found this example helpful (https://sourceforge.net/p/grampc/discussion/general/thread/21114e01aa/) . I created a vector with a length equal to the prediction horizon length (10 steps, or 0.1s, in my case). This vector is added to grampc.userparam and updated at each step in the Trajectory Generator subsystem. Then, the cost function uses the vector xd (instead of xdes), where xd's value depends on userparam and time t. Relevant code is below.
I've included an image which shows how I'm passing the desired trajectory. The "current" xd (named xdnext in the image) is still passed to the xdes port, although this shouldn't be doing anything.
This seems to be working, even if the tracking error is higher than I would like it to be. Does this all look correct?
Dear Krysten,
this definitely goes into the right direction. You should take care that if you want to cover a horizon Thor=1.0s with sampling points every 0.1s then you need Nhor=11 because the sampling time along the horizon is Thor/(Nhor-1).
You should also take care that Matlab starts counting from 1 whereas C code starts counting from 0. The element
pSys[13]is thus the 14th element in the Matlab vector. Your first step is 0 for t=0 and therefore your first xd is selected frompSys[13+2*0+0]which also evaluates topSys[13]. Probably not what you want to have.Finally, do you really have a dense Q matrix for the weighting? We nearly always use diagonal weighting matrices and then you can replace the double loop for Q'x by a single loop that multiplies the diagonal entry of Q with the respective deviation. In fact we usually use only a vector with Nx elements for Q.
Best regards,
Andreas Völz
You're right; I needed to add a +1 to the index for pSys. As for Q, I've now simplified it as a vector instead of a sparse matrix.
Perhaps this is a silly question, but since I'm not overly familiar with C, is there any way I can verify that step and xd are being correctly defined (i.e., print them out)? I ask because I initially received compilation errors regarding the index for pSys not being an integer because it depends on the quotient of two doubles, t/d_t. I played around with it and was able to get it to run using the definition of step in my earlier post, but I don't really know what I changed in probfct.c that caused it to successfully compile. The tracking performance, optimal inputs, and cost are nearly identical to the version that incorrectly held xd constant over Nhor, which makes me skeptical whether xd is really changing with each time step.
Dear Krysten,
the code
pSys[t/d_t]does not compile becauset/d_tis of floating point type but array access requires integer types. Withint step = t/d_t;you perform an implicit type cast from floating point to integer. Alternatively you could usepSys[(int)(t/d_t)]with an explicit type cast.In principle it is possible to use a debugger to go through the code step by step. While this is easy if you run GRAMPC as pure C code, it is more involved if you start it from Matlab and probably even more complicated if you run it within Simulink and requires to attach the debugger to an externally running process. My own experience is quite limited in this regard.
However, you can also add printf-statements directly in the probfct. We have in include/grampc_mess.h a macro
myPrintwith two arguments that either maps toprintfor tomexPrintfif you run your MPC from Matlab. You could use this within your problem functions. My recommendation would then be to setMaxGradIter=1,MaxMultIter=1andTsim=dtso that you only run a single gradient iteration. Otherwise, you probably get too many messages printed.Another option is to solve only the OCP in the first MPC step and look at the resulting trajectories in grampc.rws, that is, the predicted trajectories. If you run many iterations, the predicted trajectories should already follow the reference trajectory. If the desired values were constant, this should also be visible in the optimal predicted trajectories.
Best regards,
Andreas Völz
Hi Andreas,
Thank you again for your detailed advice, and I apologize for the continued questions. I've tried both approaches. If I solve the OCP in startMPC.m but stop the code before it moves to the next step, I'm able to view grampc.rws.x. Unfortunately, even with MaxMultIter = 1, MaxGradIter = 1, and Tsim = dt, these are still suspiciously similar.
I've attempted to use myPrint and printf in the problem function. While both of these print functions work if I include them in grampc_run_Cmex.c and grampc_run.c, I can't get anything in lfct or dldx (within probfct.c) to print. I consulted a colleague who's well-versed in c, but we weren't able to figure it out.
This may seem like nitpicking, but I'm building another probfct.c that uses xd in the system dynamics ffct, and it's critical that those are accurate or it could produce instability. So I'm debugging using a simpler example.
Debugging using a simpler example is always good. So what is the output of your simpler program? Does it compile? Can you see messages printed?
Yes, it compiles. When I run the startMPC MATLAB file, it prints the messages contained in grampc_run.c and grampc_run_Cmex.c after printing the GRAMPC parameters. Each message is displayed twice when I run one iteration.
Your should add a line break to all your printf statements, so
printf("%s\n",("We're now in dl/dx")). Currently you get everything in one line but I also saw "evaluateCost" which maybe your output in the lfct.Thanks! Here is my updated output with line breaks. It's not printing anything in lfct within probfct.c, though. I have the print command coded at the top of lfct as follows:
If I can get this to work, I can change what's contained in printf to show xd for that iteration. I'm concerned there's some issue with xd because if I ask my model to use the following very simple feedback control and just optimize alpha, it not only becomes unstable, but it gives radically different results each time I run it. I've tried tightening the error tolerances to no avail. The same feedback control law (using the initial guess for alpha) works in simulation without GRAMPC.
Dear Krysten,
it is quite difficult to help here with always only a part of the code available. If you get different results with each run, the reason is typically that you read from the wrong memory locations. This can happen in C, for example when variables are not initialized properly or when you read with pointers (or arrays) beyond your allocated memory. One option would be that you reduce your code to a minimal working example that still shows the problematic behaviour or I can try to create a minimal example in the next days. Since we have already done such things multiple times, I am pretty sure that the error is somewhere in your code or in how you use it from Matlab/Simulink.
Best regards,
Andreas Völz
Ok, I've simplified it to a double integrator where the two gains in a PD control law need to be optimized. The initial guesses should stabilize the system, but I'm getting the same problem as before. I re-downloaded GRAMPVC v2.2 and built this version off of the template files to ensure no changes had been made to the src, include, or matlab folders that could affect the memory. If you are able to identify where the memory issue is occurring, I will be very grateful.
Dear Krysten,
thanks for sharing the minimal example. Using GRAMPC to optimize over feedback gains instead of control inputs is a quite interesting usecase that we probably have not tested so far. Our initial motivation to add the parameter optimization was rather to enable moving horizon estimation.
It took me a while to debug your problem, but I think I found at least one main issue. GRAMPC passes the global time t0+t (running from t0 to t0+Thor) to ffct, dfdx_vec, dfdu_vec, dfdp_vec while it passes the local time t (running from 0 to Thor) to all other problem functions. Therefore, your dynamics computed step indices that were far beyond the userparam array. One solution would be to also pass in t0 via userparam and subtract it. A quicker solution is to comment the line
and change the simulation
in startMPC.m so that the global time is not updated. I guess in Simulink this would correspond to passing the constant input 0 to the tk ports of the MPC subsystem and the sysfct sfunction.
With these changes I was at least able to run the example without producing nan or inf values. Maybe there are still other errors or it could mainly be a matter of tuning, but I hope that you can continue with this.
Best regards,
Andreas Völz
Hi Andreas,
That worked! I saved t0 as a parameter per your suggestion and just subtracted it in ffct, dfdp, etc. Even in the more complex script, this solution works perfectly.
Best,
Krysten
Hello Krysten,
could you share the solution of your Simple GRAMPC Example? This would be very helpful.
Best regards,
Genc Yavuz
Hi Genc,
See attached for my trajectory tracking MPC. It's framed as a parameter optimization problem in order to test out trajectory tracking by driving the states with a PD feedback controller. The parameter optimization part itself doesn't appear to work, actually, as it never updates the initial guess. I didn't troubleshoot this aspect since it wasn't my end goal, but I hope that the trajectory aspect is helpful!