Part 3 | MPC for trajectory tracking

[@anillamichhane2180]

Such a nice presentation and demo. Thank you !

[@JaynilSheth]

Hello sir, thank you for the workshop videos. I am actually trying to implement MPC using Casadi on hardware using simulink. I have the measured states of the robot. I am unable to figure out where should I give the mpc controller the current measured states, because there always will be an error between the current state calculated using model and the actual measured current state. Should I add this error term in the objective function?

[@grandecatastrophe]

Great videos! Thanks. I just wish that the volume was a little bit higher. :)

[@MohamedWMehrez]

Thank you Harsh for your comment.

[@subhankhan6180]

Thanks a lot Dr. Mohamed for uploading this video :)

[@MohamedWMehrez]

you are welcome!

[@margauxdamien4254]

Hello Dr. Mohamed. Thank you for your videos, it helps a lot ! I still have a question: why are we obliged to define references (in matrix of parameters P) for the control variables v and omega? What if we don't have any? Aren't these references calculated automatically by the state references and the definition of the kinematic model that links these variables? Thank you in advance !

[@MohamedWMehrez]

Hi, the pre-defined reference helps in regulating the values for v and omega and also helps in finding the optimization solution quickly. MPC should work without this term but you might not like the behavior of the resulting control action. As a regularization, you can also penalize the change in the control action.

[@margauxdamien4254]

@@MohamedWMehrez Ok understood, thank you ! I have another question: how would you do if you would want to use 2 robots instead of 1 ?

[@MohamedWMehrez]

You can formulate a multi robot control problem in a centralized or distributed MPC problem. Check the below papers for more details: scholar.google.com/citations?view_op=view_citation&hl=en&user=aRNeH4QAAAAJ&citation_for_view=aRNeH4QAAAAJ:tS2w5q8j5-wC scholar.google.com/citations?view_op=view_citation&hl=en&user=aRNeH4QAAAAJ&citation_for_view=aRNeH4QAAAAJ:UeHWp8X0CEIC

[@margauxdamien4254]

@@MohamedWMehrez Hi again ! Thank you for that, would you have any example of a code ?

[@alwinaugustine5961]

Hi professor Mohamed, My name is Alwin. first of all let me give a big thanks to you for the workshop. Sir, I have some doubts regarding the trajectory tracking. 1. You are giving X_ref and Y_ref as a function of predicted time instant. So if we are giving a sinusoidal trajectory, should I use the same predicted time? since I cannot change my reference with respect current time, how can we use that predicted time for our expression? 2. If I am changing the reference trajectories(x and y) I should also change the bounds of x and y , right? so what about other bounds, since expressions for that will also change as X-ref and Y_ref changed? If I am asking a wrong question, pardon me for the inconvenience. Thank you once again for your great videos and codes.

[@MohamedWMehrez]

Hi Alwin, thank you for your comment. 1- Yes, you would plug this predicted time into your sinusoidal function to get the predicted reference. 2- If your reference for x and y is within the map boundaries then you wouldn't change the bound on x or y but rather on the vehicle speed; since this is not what happens practically because the vehicle speed is tied to the physical limits of an actual robot, you need to choose your reference trajectory such that the resulting reference speeds are feasible. All the best!

[@alwinaugustine5961]

@@MohamedWMehrez Thank you so much for the reply👍

[@madhur4190]

Great videos. Can you recommend a good book for foundations/introductions to MPC. Something along the side of how you learned it? thanks.

[@MohamedWMehrez]

Thank you! Here is a very good book to follow sites.engineering.ucsb.edu/~jbraw/mpc/

[@ArnabJoardar]

Hello Dr. Mohamed. Thank you for the Tutorial, it is proving to be indispensable. I have a dynamics equation that would be voluminous if fleshed out completely. To keep it tractable, I would use a Custom Matlab function to represent those steps and then I would recycle the script wherever I need it. How can I do this while still treating variables inside the function as Casadi Variables?

[@MohamedWMehrez]

Hi, you actually need to rewrite this function in Casadi variables then integrate with the main code

[@ArnabJoardar]

@@MohamedWMehrez Thanks for the hint. I was able to get it to work. Now, I needed to get a structured output from a function: a matrix with the top left corner containing a rotation matrix and the top right containing the position vector. Basically, a Homogeneous Transformation matrix. How can I specify this matrix as a function of certain inputs while it maintains a structure? I had used the Rodrigues formula to get a nice clean rotation matrix.

[@ArnabJoardar]

My attempt was as follows: trans_mat = Function('trans_mat',{axis_,theta},... {[rot_mat(axis_,theta) (SX.eye(3)-rot_mat(axis_,theta))*point;... 0 0 0 1]},... {'axis_','theta'},{'trans_mat'});

[@ArnabJoardar]

Hello doctor. I tried out the function: % Define Homogeneous Transformation matrix theta = MX.sym('theta'); axis_ = MX.sym('axis',3,1); point = MX.sym('point',3,1); axis_cross_mat = Function('unit_axis',{axis_},... {[0 -axis_(3,1) axis_(2,1);... axis_(3,1) 0 -axis_(1,1);... -axis_(2,1) axis_(1,1) 0]/norm(axis_)},... {'axis'},{'axis_cross_mat'}); rot_mat = Function('unit_axis',{axis_,theta},... {eye(3)+axis_cross_mat(axis_)*sin(theta)+axis_cross_mat(axis_)^2*(1-cos(theta))},... {'axis_','theta'},{'rot_mat'}); trans_mat = Function('trans_mat',{axis_,theta,point},... {[rot_mat(axis_,theta) (eye(3)-rot_mat(axis_,theta))*point;... 0 0 0 1]},... {'axis_','theta','point'},{'trans_mat'}); and it seems to work fine. One thing that I noticed is that the function object has the syntax: f = functionname(name, arguments, ..., [options]). Then, inside 'arguments', can I have a complete function like: function [mat] = homo_trans_mat(axis,theta,point) %UNTITLED We calculate the rigid body motion transformation matrix here % Detailed explanation goes here mat = zeros(4,4); % Angular displacement component len_axis = norm(axis); unit_axis = axis/len_axis; axis_cross_mat = [0 -unit_axis(3,1) unit_axis(2,1);... unit_axis(3,1) 0 -unit_axis(1,1);... -unit_axis(2,1) unit_axis(1,1) 0]; rot_mat = eye(3)+axis_cross_mat*sin(theta)+axis_cross_mat^2*(1-cos(theta)); mat(1:3,1:3) = rot_mat; % Linear displacement component mat(1:4,4) = [(eye(3)-rot_mat)*point;1]; end sitting inside? You might notice that I had to convert the above function into several functions in symbolic form.

[@MohamedWMehrez]

Hi, I didn't get to this level of using functions before, but it seems like you are doing it right.

[@moumainacereddine7629]

Thank you, Sir, for the video. However, I have a question to ask. I have a pre-trained TensorFlow model for making predictions and I would like to use it to apply Model Predictive Control (MPC) using CasADi. I read that only PyTorch models can be used with CasADi. Is there another way to use a TensorFlow model with CasADi without converting it to PyTorch?

[@MohamedWMehrez]

Thank you for your comment. I am not sure to be honest as I haven't tried it before.

[@pranavinani94]

Thank you for the excellent lecture series. Can you comment on how the formulation would change if the problem changed from trajectory tracking problem to a path tracking problem?

[@MohamedWMehrez]

Thank you for your comment. check out this paper; it shows you how to formulate a path following problem and solve it using MPC. www.sciencedirect.com/science/article/pii/S2405896317313733

[@Leedimy]

Dear Prof. Mohamed, Thank you for providing this lecture. In 15:55, could you tell me how to set the constraints? You've mentioned that "3" is the number of state variables. In my case study, the states x is 2x1, controls u is 1x1, and N=2; I use a linear model to implement it. Those matrices, such as lbg, ubg, lbx, ubx should be 8x1 dimensions, the compiler suggests that. I don't know how to solve this bug. Thank you,

[@MohamedWMehrez]

Hi Song, Thank you! I think you simply need to replace the 3 with 2 (your number of states). I suggest though that you first go through the code for the provided model and understand how the ranges for these vectors are like. Then, you should be able to fix these numbers and adapt them to your problem or even to any other problem with different dimensions.

[@Leedimy]

@@MohamedWMehrez Thank you, I got it. It is not as simple as changing "3 to 2", I need to understand the vector size. I fixed it, successfully.

[@nicktaonemay]

Hi Dr. Mohamed. Thank you for the great lectures. This MPC is designed for discrete-time. I would like to ask you how can I calculate the integral for continous-time system, similar to your papers. Thank you.

[@MohamedWMehrez]

Hi, thank you for your comment. Please check the second part of this video series where RK4 method is used to integrate continuous time systems very accurately.

[@jeremigraveline3065]

Thank you for the great videos. I was wondering if neglecting the control reference (Uref) would still make this MPC a feedforward controller. It would still use the model to make prediction of states that are then used as initial guess for the NLP. I'm using as a Xref a list of waypoints that are not a function of time but only states to reach one after the other. Uref then becomes hard to get. Would you think (Uref) would still be an important parameter to calculate? Thank you !

[@MohamedWMehrez]

Thanks for your comment. I see this implementation as a successive point stabilization in which you update x_ref every time step and keep it constant over the prediction horizon. This is different from trajectory tracking in which x_ref gets updated within the same prediction horizon. There is actually nothing wrong with what you suggested, but we can't clearly call it trajectory tracking. Moreover, If x_ref gets updated within the same prediction horizon while u_ref = 0 that's also still tracking, but with a feedforward control of zero. You can also avoid penalizing the control action in the cost function altogether, but you will lose regularization of the control and the resulting OCP can be a little harder to solve.

[@jeremigraveline3065]

@@MohamedWMehrez Thank you for you answer, I really appreciate the help. I'm not sure to understand how u_ref makes the feedforward controller. In the case where, as you say, it is not penalized in the cost function, a prediction of controls and states will still be done and set as the initial guess at the next timestep. Doesn't that make it a feedforward controller?

[@MohamedWMehrez]

@@jeremigraveline3065 u_ref is basically the control value that when you apply to the system, the system would ideally follow its reference x_ref. I see what you mean by not penalizing the control at all and use the previous solution for initialization. To be honest I don't think that this can be considered as a feedforward control especially for the first step in MPC where you typically don't know a good guess for u. I suggest that you check the literature on how a feedforward controller is implemented for the system you are considering.

[@jeremigraveline3065]

Thank you for your answers, I'll keep studying the case as you say, I get what you mean.

[@quangnhatle2512]

Hi Professor. Thank you for your great video. I have one question . Suppose I have a completely random car track which has left and right boundary. This track also has centerline coordinates in each time step. I want to design a car model that can follow this track and still inside both boundaries. If I only have this car track 's x and y centerline coordinates in each time step, how do I find X_ref and U_ref ?

[@MohamedWMehrez]

Hi, thank you for your comment. I think you may fit a function to your boundaries and then you would have a time varying constraints. Never tried this though.

[@SbAlmagro]

Hi Dr. Mohamed. As you have pointed out, the penalty matrices Q and R have a greater effect in the performance of the MPC. Choosing the right values might be hard tasks. I am wondering if those could be considered as optimization variables?

[@MohamedWMehrez]

Hi, Theoretically you can, but it would be hard to enforce a particular performance, because the optimizer will chose the values of Q and R that minimizes the cost function. Also you will need to be careful when chosing their limits as if you allow them to be zero, that's gonna be the optimizer choice.

[@DDONISBABBURGER]

Thank you for your excellent lectures. I'd like to research the formation control of submarine using MPC. Could you recommend some sources or papers about Formation control? I appreciate your helpful series again.

[@MohamedWMehrez]

I believe that there is a ton of papers on such topic; just google it and I am sure you will find many useful ones. I have also some papers published on this topic, you can find my publications here scholar.google.com/citations?user=aRNeH4QAAAAJ&hl=en

[@yifanxue4144]

Here is a problem that if the obstacle is dynamic and changing each iteration. On Tue, Oct 27, 2020, 9:59 AM Xyf wrote: Dr. Mohamed, Thanks a lot for your great videos. With the help of your explanation, I successfully applied the relevant methods to obstacle avoidance and trajectory tracking of ships. But I ran into a problem. The obstacles in the video is fixed, such as g = [g ; -sqrt((X(4,k)-obs_x)^2+(X(5,k)-obs_y)^2) + (rob_diam/2 + obs_diam/2)]; If the position of the obstacle is dynamic and changing with each iteration, and I have the trajectory of the obstacle, can I still use the original method to solve the problem? In other word, I want the the g function following g = [g ; -sqrt((X(4,k)-obs_refx(mpciter+1))^2+(X(5,k)-obs_refy(mpciter+1)^2) + (rob_diam/2 + obs_diam/2)]; How can I solve this problem? I try to reinitialize the g function and 'nlp_prob' every iteration in the loop, which will be very slow. On 10/30/2020 05:00，Mohamed Mehrez Said wrote： In case your obstacle has a known trajectory, then you should consider this trajectory waypoints as extra parameters in your optimization problem. Therefore, similar to how we defined the reference trajectory for the robot as a series of parameters being fed to the optimization problem every time step, you should also do the same for the obstacle trajectory and feed its time-dependant waypoints every sampling time to your optimizer. What you are doing currently is regenerating a new optimization problem every time step, and this is for sure slow. On 02/11/2020 20:00，Xyf wrote： Hi Mohamed, Thanks for your patient reply. I understand the problem now, if the obstacle's trajectory is independent with time, I should fed it to the opt every time and it will cost much time. If the obstacle's trajectory is in a functional form such as another ship's fixed route f2(t), I can add the obstacle function into the original dynamic function X. By this way, the the problem can be solved as g = [g ; -sqrt((X(4,k)-X(add,k))^2+(X(5,k)-X(add+1,k)^2) + (rob_diam/2 + obs_diam/2)].

[@MohamedWMehrez]

You seem to know exactly what has to been done, so what is your question?

[@muhammadhd8407]

Hi Mohamed, I have read that we can use MPC for trajectory planning. In this case, could you please tell me what is the difference from MPC trajectory tracking implementation in Casadi?

[@MohamedWMehrez]

It all depends on the objective function, the optimization variables, and the optimal values of the optimization variables and how you nare going to use them. For mpc, you are normally interested in the feedback control actions. In trajectory planning, you're normally interested in the state prediction.

[@fayyazpocker840]

Thanks a lot for your videos. They are really good and gives a good understanding of MPC. However, I have some doubts regarding the implementation of MPC on a real robot and it would be great if you could clear it. 1. How do I allocate the uncertainty in the state estimation from lets say an EKF to my state space model of MPC ? Also, If lets say weight of the robot is dynamic while moving, how can I add that information to the MPC for a good prediction ? 2. In the example you have described, what would be the effect if i include only x and y in the cost function and do not involve theta. Wouldn't the robot still be aligned properly on the path if the prediction horizon is big enough ? 3. Wouldn't adding weights to the control input to follow the reference control input act against the correction. Lets say if i am tracking an arc and I penalize the correction in angular velocity, wouldn't it be more like acting against the angular correction even though there is a feedforward control action? 4. How do I effectively choose the sample time between each prediction step ?

[@MohamedWMehrez]

Thank you for your comment: 1- There are many studies on stochastic MPC which consider uncertainty of the measured state. Nonetheless, for small uncertainties, I believe that nominal MPC should work fine because of its receding horizon nature. Please note that I used only the kinematic model in the simulations. If you would like to include a changing weight, you will need to derive the dynamic model of the system and use it for prediction. Nonetheless, I believe that most of the research robotic platform follow pretty well the kinematic model even if there load is varying.

[@MohamedWMehrez]

2- Yes it would. Given that you are also penalizing the control actions.

[@MohamedWMehrez]

3- in some sense yes; however, keep in mind that the state is also penalized in the cost function. In this case, if you have more weight on the state error than the control error, then your robot will want to approach the reference trajectory quickly then tracks it (that's when the feedforward control is helpful).

[@MohamedWMehrez]

4- it depends on different factors; for example, if your system dynamics are fast, e.g. a flying robot, you may want to have a shorter sampling time than a slow dynamics system. Also, the approximation model you use for simulating the system, e.g. if you use Euler approximation, then a short sampling time is needed for a better prediction accuracy. While if you use RK4 integration, then you may relax this a bit. Finally, perhaps the prediction horizon you use could also be a factor, e.g. if you have a very short sampling time and a short prediction horizon, your prediction will see the future for only a limited duration and this affects the performance of your controller.

[@fayyazpocker840]

@@MohamedWMehrez 2. why is that i should penalize the control actions? If not does it make oscillations due to aggressive correction ?

[@paulduchane7278]

Hello, thank you so much for this workshop. I watched the first video and this one. I was trying to test the circular path as the one in your paper "stabilizing NMPC of Wheeled Mobile Robots Using Open-Source Real-Time Software ". There is a few points I do not understand. I wanted to change the linear path in your code example by the circular path. I changed the x_ref and y_ref such as x_ref = 0.3+2sin(0.25 t_predict), y_ref = -2.3+2cos(0.25 t_predict). But by what should I replace u_ref? is there anything else I should change? thank you for you response

[@MohamedWMehrez]

Thank you for your comment. From xref and yref you should calculate an expression for theta_ref = atan (yref/xref)... Looking at the system equation it's clear from the third equation that w_ref = (theta_ref)_dot... Then, looking at the first two equations you should be able to find an expression for v_ref as a function of xref and yref and their time derivatives. This expression can be found by squaring then adding the first two equations of the system model. I think this will lead to v_ref = sqrt(xref_dot^2+yref_dot^2).

[@paulduchane7278]

@@MohamedWMehrez Thank you for your answer. There are some behavior I don't understand. For example, I tried the linear trajectory by changing the y_ref : y_ref = -3 and initialized it with 0 (x0=[0; 0; 0]) and it converge towards the value y = -2. It should converge to -3 but here not. Same observation for y_ref = 6 and initialization with x0=[0; 5; 0]. Then for the circular trajectory, with an initial condition of x0=[0; 0; 0], it starts to converge moving clockwise in the circular trajectory (so everything is normal and ok), but after reaching the point at the quarter of the circle (x = 2.28, y = -2.05) , it turns right and goes straight on (following the line y=-2), and when it reaches the opposite side of the circular trajectory (x = -1.6, y = --2.05) , then it converges again to the trajectory. It was like it was following a half circle trajectory . I really dont' understand why there is such behavior for the circular trajectory and why for some cases of linear trajectory it doesn't work. Maybe you have some explanation? Thank you again for your time and your help.

[@MohamedWMehrez]

Hi Paul, Concerning your issue, have you also changed the constraints on the possible x and y position your robot can have? if your reference is outside the map margins then the robot can't reach that reference :D ... so your reference has to be consistent with the map margins.

[@paulduchane7278]

Oh yes... I feel so stupid. Thank you!

Did you figure this out? I tried it too, but it's not working. It's not drawing a circle but some wierd shape.

[@ibrahimseleem]

Thanks Dr. Mohamed for this video. If the reference trajectories are real points with time (not function like your code example). How can I modify the reference part?

[@MohamedWMehrez]

In this case, you can lookup your vector of reference points by the sampling instant and pass the reference points (for this time instant and the following instants depending on the prediction horizon) as parameters everytime step. very similar to how you update the feedback every time step.

[@ibrahimseleem]

Thanks a lot. I tried to implement the same idea, but I faced a problem because of the prediction Horizon (N). If the system has 6-states and 3-control input. while the reference points are 1124. while(mpciter < sim_tim / T) %Mpciter=1124 args.p(1:6) = x0; for k = 1:N % N=10 % load reference vectors args.p(9*k-2:9*k+3) = [x_ref (iteration_relation), y_ref (iteration_relation), ,...etc]; end end In for loop, at each prediction iteration, arg.p=96 value (states+N*(states+control)). The problem here is that the code will stop before reaches the final mpciter iteration number. what is your suggestion to compute iteration_relation raltive to mpciter?

[@MohamedWMehrez]

You can simply repeat the last reference multiple times until you reach the end of your trajectory.

[@ibrahimseleem]

@@MohamedWMehrez Thanks a lot. I modified the code to deal with iterative reference points. Regarding the reference control signal, in presentation, the model is simple so it is easy to compute the reference control signal. but what are the best choices to calculate the reference control in a complex model? Example: x_dot=J*u. J is 3*3 matrix with nonlinear elements u.

[@MohamedWMehrez]

@@ibrahimseleem you can simply calculate u as u = J^-1 * x_dot_ref

[@christopherreddick1802]

Dr. Mohamed, Great videos. In github it appears that the contents of "Draw_MPC_tracking_v1.m" actually contain the same contents as "Draw_MPC_point_stabilization_v1.m". Thank you.

[@MohamedWMehrez]

Hi Christopher, thank you for your comment. The two files are similar but not exactly the same. In the one for tracking I am plotting the reference trajectory and changing the margin of the resulting figure.

[@christopherreddick1802]

Hello Dr. Mehrez. Currently, the first line of the Draw_MPC_tracking_v1.m on the github site is the following: function Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam). Is this what you intended? Thanks for the great content that you have created.

[@MohamedWMehrez]

Hi Christopher, It is not intended, but it won't affect the performance of the code. Normally in Matlab you would name the m-file with the same function name, but for some reasons I missed doing that. Thanks for your observation.

[@antonete125]

Do you think it is convenient to add the end term (or Mayer term in other biographies) to the objective function for trajectory tracking purposes? Thanks in advance!

[@MohamedWMehrez]

Thank you for your comment. A terminal cost can be added for the point stabilization case as well. Choosing the terminal cost properly and perhaps a terminal region constraint is one way of guaranteeing stability of the closed loop system. There are also methods for ensuring stability without the terminal cost altogether, see, e.g. www.sciencedirect.com/science/article/pii/S0921889019306232 and the references therein.

[@welidbenchouche]

hello doc, if i wanted a circular path, say for example a circle with r radius, i should set x_ref = r*cos(t_predict) or x_ref = r*cos(theta) ?

[@MohamedWMehrez]

Check the circular trajectory in this paper www.researchgate.net/publication/271549212_Stabilizing_NMPC_of_wheeled_mobile_robots_using_open-source_real-time_software

[@RobinHelsing]

Is it possible to implement this in Simulink somehow? I am doing a thesis regarding PMSM where I would like a NMPC. Which is a cascade coupled system with 1 out speed control loop, and two inner control loops for the current.

[@MohamedWMehrez]

check out Casadi blog for simulink examples web.casadi.org/blog/

[@welidbenchouche]

Great video doc. But I did everything like you did. And i get an error in line 128

[@MohamedWMehrez]

Try the code that's already uploaded on the GitHub folder.