alexliniger / mpcc Goto Github PK

Thanks for sharing this, its really neat. Is the paper on the 1:43 scale implementation publicly available?

Hi,

I have carefully followed your work and your cooperation with AMZ Driverless team. I managed to implement the Udacity MPC algorithm on my robot but i found conversion of MPCC from matlab to C++ very difficult especially the theta function.

I was wondering how did you manage to implement the MPCC algorithm on RC car and driverless car without having the C++ version code.

Hello, Alex!

Yours plotBox method in class Plotting does not work properly, because you need to divide body_xl, body_xw, body_yl and body_yw by two before appending and subtracting them from X and Y coordinates of the car.

Regards,
Pavel.

Hi
I followed your work carefully and implemented it by using matlab-ros interface. But i failed to control the car well even for going straight forward. I wonder if your algorithm of the simulation is the same as the algorithm of your real car. Have you add some closed loop control method such as PID or just using the open loop MPC?

Waiting for your reply. Thank you!

Hi @alexliniger

In this video (https://www.youtube.com/watch?v=JoHfJ6LEKVo), do you use the same approach as in this repository. Are the dynamic obstacles considered as freezed in time every sampling instant?

Kind Regards
Pranjal Biswas

Hi @alexliniger ,

I was trying to work with slack variables to soften the track constraint. I was referring to the following book regarding slack variables (https://web.stanford.edu/~boyd/cvxbook/bv_cvxbook.pdf, page 131, Eq 4.7). It says that to incorporate slack variables a new optimization variable is introduced, and the inequality constraint is transformed to an equality constraint. Although if I understand this correctly, will this not lead to the ego being always on the edge of the track? Because irrespective of how high the weight on slack variable be in the cost function, it will always try to satisfy the equality constraint (Eq 4.7). Is there anything which I am missing in my understanding? Looking forward to hear from you.

Kind Regards

Pranjal

Hi,

Hi @alexliniger ,

I have been trying to implement MPCC for a while now, but I have an issue with the contouring error that it is exploding as shown in the Figure. Here it can be seen that lateral position of the real vehicle (red) and the predicted position (yellow) is deviating from the reference path at that arc-length (green).

I know it can be highly dependent on the way I have implemented things, but can you give some points from experience on where I might be going wrong which is making the error dynamics unstable. I am just using MATLAB NMPC toolbox to optimize for acceleration and steering rate without any input rate costs and constraints at the moment. Also, I am sure that the MPC vehicle dynamic model is correct from first principles and also it follows the high fidelity real world system response as can be seen in the plot. Also, the error cost is as simple as:

J = sum(qc.*(E_C.^2)) + sum(ql.*(E_L.^2)) - sum((rv.*V_P).*Ts); %sum over full horizon

so I doubt if that is wrong as well. Do you recommend looking at any other critical points which may be causing the issue? Looking forward to hear from you.

Kind Regards

Pranjal

I bought a totally same car as you described in your paper, but I got into trouble when identifying the parameters of the car model:

1. The weight of the car is 186.5 g, which is much smaller (41 g) in your code, but the length and width is the same. I wonder if the parameters in your code is just an example case or true value you measured?
2. I read the paper "Analysis and control of high sideslip manoeuvres", which denoted the approach of identifying the Pacejka Tire Model. However this approach needs too much area, which cannot be realized on the track. Could you please share the details of the way for identifying the parameters?

Waiting for your reply. Thank you.

Hi,

I'm working on performing the Taylor expansion of the first two terms of equations 13a to get the first two terms of 14a. I'm using an automatic differentiation package to get the c_k and Gamma_k terms in equation 14a however I'm finding that the quadratic part is not necessarily positive semi-definite. Did you employ any specific techniques to ensure that the quadratic part was psd?

Also, just a quick question regarding notation, in the first two terms under the summation in equation 14a, should the vectors [x_k theta_A,k] be interpreted to represent [x_k-x_0,k theta_A,k-theta_A0, k], where the 0 subscripted terms represent the points about which we are Taylor expanding? Or am I misunderstanding?

Thanks!

I follow how the model dynamics are linearized/discretized to form Ad, Bd, and gd. However, could you provide some more detail about the following scaling step that seems to lift the problem:

% constructing augmented system with state-input scaling
    Ak = [MPC_vars.Tx*Ad*MPC_vars.invTx MPC_vars.Tx*Bd*MPC_vars.invTu; zeros(nu,nx) eye(nu)];
    Bk = [MPC_vars.Tx*Bd*MPC_vars.invTu;eye(nu)];
    gk = [MPC_vars.Tx*gd;zeros(nu,1)];

I don't quite follow why we increase the dimension of x by nu.

Hi,

I've used the Udacity's MPC C++ implementation that's a path-following MPC, and the results look like this: https://youtu.be/9LhFnp30Ddk
The green marker denotes the waypoints that define the path that the robot should follow, the white marker are the waypoints used to fit a polynomial (whose coefficients are then passed to the optimizer), and the blue marker are the next steps that are by-product of the result of the MPC. The implementation uses Ipopt as its optimizer.

But I'm facing a peculiar problem: whenever I approach a 90 deg. turn, I need to clip the waypoints to which I fit the polynomial (you can see that for example at this moment) because in the robot's coordinate system this is no longer a function, and the polynomial fitting fails.

Question 1: do you know of any tricks that might help me with that? I would like to be able to follow 90 deg. turns, U-turns, chicanes, and many other types of paths that will cause this problem, and clipping the path to the nearest "still-a-function" segment is hardly a satisfactory solution.

MPCC handles this problem more elegantly. If I understand correctly, in MPCC you're using splines to encode the track, and based on those splines you define the boundaries within which the robot needs to stay when finding an optimal solution. This is perfect, and for my purposes this would be ideal, however, I don't have MATLAB so re-implementing MPCC to C++ would be a challenge.

Question 2: are you planning to implement MPCC in C++? From my perspective, and my current understanding of MPCC, the getNewBorders.m, and also the splines library seem to be the hardest parts.

Hi,

Is the solver used here same as the one used within FORCES PRO. If not, does it run at the same speed as mentioned in the paper which is referred to in this repository?

Kind Regards
Pranjal Biswas

Hello, I have a doubt, can you point out my mistake
In the generateNewInitialGuess function, the control variables are set to 0; the initialGuess state variables of N steps are obtained, and the sqp_mixing_ variable and the optimization result are used to obtain an average value. my question is,

1. why mix the optimization result with the very inaccurate initialGuess；

2. Is initialGuess calculated only based on the reference track? If so, in the subsequent cost calculation, the error of each step N is also calculated based on the initialGuess and the reference trajectory. Shouldn't the error always be 0? Take the code as an example to illustrate:
   CostMatrix Cost::getHeadingCost(const ArcLengthSpline &track, const State &x, int k) const
   {
   // get heading of the track
   const Eigen::Vector2d dpos_ref = track.getDerivative(x.s);
   const double dx_ref = dpos_ref(0);
   const double dy_ref = dpos_ref(1);
   // angle of the reference path
   double theta_ref = atan2(dy_ref, dx_ref);
   theta_ref += 2.0 * M_PI * std::round((x.phi - theta_ref) / (2.0 * M_PI));

   .......
   }
   the input State x is based on the reference track, and theta_ref is based on the track (x.s), so the x.phi == theta_ref all the time??

Hi @alexliniger ,

I see you parametrize the track using arc length parametrized cubic splines. Is there any specific resource/paper I can refer to on how to achieve such a parametrization and read about it. Please let me know.

Kind Regards
Pranjal Biswas

which part of the code considers the lag or time gap between the vehicle model and the code ?

Hi @alexliniger,

While going through your paper referenced in this repository, I see that in Equation 13a, in the cost function you have change in inputs (delta u_k) and change in velocity along the path (delta v_k). Please correct me if I am wrong, but as far as my understanding of MPC goes variables in cost function can only be terms related to the states and the inputs of the system, while delta u_k or delta v_k are neither states nor the inputs of the system (Equation 12c and 13c). So how do these two terms in the cost function fit within the optimization framework here?

Kind Regards
Pranjal Biswas

Hello,
I am actually checking the project, but i was thinking about steering constraint. Sometimes, I can see that the car make turn with a high steering angle (20°) and a high speed. I think it s impossible that a car can do that, so I have a little question :
Can we manage the maximum steering angle using your method ?

When i tried make the project of C++ version, an error occurred. but ./MPCC operated normal.

The error message was mainly about " ~./config.h:24:10: fatal error: Eigen/Dense: No such file or directory
#include <Eigen/Dense> compilation terminated.CMakeFiles/MPCC.dir/build.make:62: recipe for target
'CMakeFiles/MPCC.dir/main.cpp.o' failed " .

I want to know if this status was properly. Anyone could help me? Thank you very much.

Hi,

I read the paper referred to in this repository. In the video I see cars making turns at high speeds without spinning out. Also if I am correct the states of your system are (x,y,phy,v_x, v_y, omega,theta). So do you put any specific constraint on the states to guarantee stability at the limits of handling for the MPCC approach, or stability is not guaranteed? Looking forward to hear from you.

Kind Regards
Pranjal Biswas

Hi @alexliniger

I am using acado to reproduce the MPCC, it seems that it can simply run in the global coordinate system, but when phi = M_PI produces weird problems like the figure below.

So I plan to convert the coordinate system of the mpc problem to the vehicle coordinate system, but it seems that the updateInitialGuess step needs a lot of coordinate system conversion. I have not tested whether it is feasible, I would like to ask if you have any good ways to reduce the transformation of a large number of coordinate systems when converted into a vehicle coordinate system?

Best,
Tairan

Hi, everyone,
When discretizing the dynamic model of the car in code of C++ version, Tayloy series expansion had first been applied to get the linearized equation \dot (x) = A x + B u + g. This is the way we often see to linearize the non-linear funnction. Then in some books, "\dot x = [x(k+1) - x(k)] / T" was used to get disctretize the model, which was obviously different from what had been done in C++ code .So I am confuzed about why the matrix exponetial of A, B and g was obtained to get the discretized model? So could anyone explain how to derive it.

Update: Created by mistake

when I tried to run ./install.sh to install the project of c++ version, an error occured.
The error info was mainly about "CMake error in hpipm/test_problems/CmakeLists.txt". In fact, there was a target "hpipm" containing relative path in its INTERFACE_INCLUDE_DIRECTORIES: '../../BLASFEO/lib/include'. I guess maybe it was because of the version of cmake, I couldn't use the relative path in CmakeList.txt. And I was just not able to revise the relevant code. Anyone could help me? Thank you so much.

Hi, Alex
I have 2 questions:

1. In the track constraints of the MPCC problem quadratic approximation:
   
   for any k, the track boundary (corridor) is computed according to the theta of the corresponding initial guess point. But obviously, the solution might be different from the initial guess, so at the kth point of the solution, the theta changes, thus the track boundary is actually not the value used in our constraints.
   So it seems that even when the track constraints are satisfied, the predicted trajectory may have collision with obstacles? How do you deal with this situation?
2. In obstacle avoidance, the vehicle is treated as a point without considering its heading. Does this matter when the corridor becomes narrow? Sometimes even if the point stays within the corridor, part of the vehicle might be out of the corridor because of its length and heading.
   Thanks!

Could you please explain the purpose of the adjustment present in https://github.com/alexliniger/MPCC/blob/master/Matlab/DiscretizedLinearizedModel.m#L54-L62. Thanks

Hi @alexliniger ,

If I understand the paper referenced in this repository correctly, arc-length of the parameterized path acts as one of the states for MPCC. And at each time step all states need to be initialized using the actual current states of the vehicle to predict over the prediction horizon. What is the best way to initialize arc-length (theta)? Is it advisable to just take the previous time step predicted arc-length, or should one calculate the exact optimal arc-length by projecting the current vehicle position on to the parameterized reference path i.e. solve Eq. 8 at each MPCC time step just for initialization of one state? Please let me know.

Kind Regards

Pranjal Biswas

Hi @alexliniger,

I was working out the math to go from Eq (13a) to (14a). In that while linearizing the errors it requires to calculate derivatives of the arc-length parametrized reference path w.r.t the arc-length. Also, the reference path is parametrized using splines that are piecewise polynomials. So. if linearizing at points away from the knots of splines it is possible to select a specific polynomial, although if we linearize at points very close to the knot derivative of which polynomial should be taken? Because it might happen that $\thata_{A}$ may lie in next polynomial of the spline while $\theta_{P}$ in the previous polynomial. Will this not result in wrong approximation of the error? Or am I understanding it incorrectly?

Kind Regards
Pranjal Biswas

Hello, Alex

I have been trying to use the parameters of the full_size model in the Matlab implementation to run the C++ implementation, but the solver exit flag=2.

I did the normalization as you did in the Matlab implementation.

Have you try the same thing or can you give me some advice ?

Hope for replies, thanks.

Best,

Lixiang

Hi @alexliniger

In the above link you discretize the affine term (gc), but I do not understand how the math follows here. Do you have any reference for the same?

Regards

Pranjal

Hi @alexliniger ，

I modified the model parameters and boundary conditions, and kept the cost parameters. Try to test on a circular track, but after 600 iterations, the delta change is extremely small. Do you know the reason?

Thanks,
Tairan

Hi @alexliniger

I see that in the code, you approximate the arc length by the straight-line distance between two points.

Thus, If I understand the code correctly, the arc length error will be larger when the distance between the two points is large. So I have one question:

1. Whether the distance between the input reference path points should be as small as possible.

Thanks
Tairan Chen

I changed the track to trackMobil, the solver failed. But it works well when loading track2 as the track. Could you please help me to solve the problem?

Hi @alexliniger ,

I don't know if its the right place for this query but am really curious about it, so I will put it here so that it might be useful for others as well.

I have been reading on Model Predictive Reference Tracking Control and Model Predictive Path Following Control (MPFC) both. While you have used MPFC in a MPCC framework in the paper here, you mention that only issue with MPCC arises in cases of significant model mismatch and no more limitations other than that have been highlighted in the paper if I am correct. Since my understanding of MPFC is quite new, it is very fascinating to me in the sense that a optimized velocity profile can be evaluated using just a reference path parametrized by arc length. This makes me wonder than is at all required to generate reference trajectory profiles for applications related to autonomous driving which either requires time consuming non-linear optimization or are many times hand curated which is not optimal. Thus in that sense MPCC approach totally outweighs reference tracking approach. But since I believe in the philosophy of "No Free Lunch", I believe there might as well be a drawback to using an MPCC approach, although I am not able to get my head around that. So, my question here will be in your opinion can MPCC be sure shot solution to low level motion planning for applications of autonomous driving (urban and racing both), or there is still a drawback to it which will limit its usage?

Really looking forward to hear from you.

Kind Regards
Pranjal Biswas

I am trying to use the full size branch for my application. I have imported waypoints externally. But i am getting an message after the computation has been done and the trajectory generated is does not adheres to the curvature present in the waypoints :
this is message
ipm arg size = 0
comp time = 0.001643
exitflag 2
ipm iter = 8
dist too large
Could you please guide me and tell me other changes needed for a full application on full size car simulation.

Hi @alexliniger

After I customize my track and bound, I try to modify the model but found that the exitflag of the solver was 2. I think it should be caused by the improper vehicle model parameters I set. Would you like to ask if you have full-size vehicle model parameters for simulation?

Waiting for your reply. Thank you !

Tairan Chen

Hi!
I am trying to implement the MPCC in C++, but I don't understand the discretization part. Where can I find the proof of the discretization method used in the code?
Thank you.

Hi, everyone,
In C++ code, Data was actually wrapped in many places. Overall, it can be maybe about 3 ways, if I am not mistaken.
First,

Hi,
https://github.com/alexliniger/MPCC/blob/3ec7805e7a176c57250c1a414a6d2b871e472d74/C%2B%2B/Interfaces/hpipm_interface.cpp#L60-#L63
here you get 4 matrix fot the cost of slack variables.
According to the explaination here in

The way points in the C++/Params/track.json already consists of the racing line (X and Y co-ordinate ) as input which the RC cars follows. Shouldn't the trajectory (X and Y co-ordinates ) be calculated by the cost functions as an output ?

Hi Alex,

I'm working on an implementation of the theory in Cautious NMPC with Gaussian process dynamics for autonomous miniature race cars and have been having a hard time getting it to work in FORCES PRO. Is there a repo for this? I shot you a couple emails as well but I thought I'd try here as well.

Thanks!
Sean

Hi, Alex
Do you have the comparison results of the solver performance between Hpipm and FORCES Pro?
Which is better?

Thanks.

Hi @alexliniger

Thank you very much for providing the C ++ version, which will be of great help to my study and research.

One of the little things I want to ask you. When I build the C ++ version in Ubuntu 16.04 cmake 3.5.2 environment, it prompts me that the cmake version is too low. I tried to modify the minimum version limit of the CMakeLists.txt file, and the compilation was successful.

So does different cmake versions affect the performance of the MPCC?

I wish you a Happy New Year 2020 !

Best,
Tairan Chen

Hello,I find that when i run the ./MPCC, there is a error “Illegal instruction (core dumped)” after "bounds initialized", but everything is nice before.I don't know how to solve this problem.Thank you.

Hi @alexliniger ,

I was trying to tune the weights of contouring error (qc) and lag error (ql). I understand that lag error weight should be large to have accurate error approximation, but in what order of magnitude the weights should ideally be? Any good starting point for tuning?

Kind Reagrds
Pranjal Biswas

Hi alexliniger,
https://github.com/alexliniger/MPCC/blob/3ec7805e7a176c57250c1a414a6d2b871e472d74/C%2B%2B/MPC/mpc.cpp#L238#L239
could you please tell me what n_no_solves_sqp_ means?
https://github.com/alexliniger/MPCC/blob/3ec7805e7a176c57250c1a414a6d2b871e472d74/C%2B%2B/MPC/mpc.cpp#L244-#L251
Also I am so sorry I could ot understand the logic of the code here? Hope you could explain it to me? I believe it also has something to do with the meaning of n_no_solves_sqp_ . But I just am not able to get it.
Thank you so much first.

@alexliniger Hi, the C++ version is an implementation of this paper "AMZ Driverless: The Full Autonomous Racing System", but I noticed that the solver used is HPIPM, which is a QP solver. So the C++ version does not solve the non-linear problem directly(as described in the paper), but uses the same linearization method as the matlab version("Optimization‐based autonomous racing of 1:43 scale RC cars"). Am I right?
also, I wonder is it efficient enough to solve the NLP directly.
Thanks!

Hi Alex,

I'm currently new to optimisation theory and I don't quite follow why we need to normalise the QP problem as seen in your C++ code. Does this result in obtaining the solution faster ? Additionally, how did you get the normalisation parameters ? Thank you in advance.

Regards,
Dennis

Hi alexliniger,

Hi @alexliniger ,

In the paper you define tracks constraint using a pair of half-spaces over the prediction horizon. From what I understand these constraints will vary over the prediction horizon and are not really known beforehand. I have few questions in relation to this:

1. Just to confirm halfspaces represent linear equations of line, parametrized by state X and Y (position of the vehicle)?

2. How exactly can i formulate constraints which will vary over prediction horizon and are not known before hand, as it will depend on position of the vehicle at any given prediction step. I am only aware of defining constraints and then solving the optimization problem, any references?

3. The paper (Figure 4) says the green points on borders are projection of the center line points. How exactly is this projection evaluated, it is not very clear to me.

Please let me know.

Kind Regards

Pranjal Biswas