[QUESTION]: Path following MPC and C++ implementation of MPCC

[MTDzi]

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.

[alexliniger]

As I am not familiar with the Udacity MPC implementation I can not directly help. I also don't understand what you mean with "robot's coordinate system this is no longer a function, and the polynomial fitting fails". Is there an article describing the controller.

I already implemented large parts of the MPCC in C++, but still needs some work. The spline library is actually not too hard to implement, I even optimized it such that it works with unevenly distributed points on the centerline. Something the current implementation really does not like. I currently don't plan to rewrite the getNewBorders as there are better ways to solve this step especially if the obstacles are moving.

Maybe the code works in Octave if you want to play around a bit with the code. There are no special functions involved, so there is hope that it will work.

[MTDzi]

Hey Alex,

Thanks for the response!

I'm terribly sorry for not following up right away, I must have missed the notification that you've answered my question.

In the Udacity MPC implementation there is a transformation of the waypoints from the global coordinate system into the car's coordinate system. Once that is done, a polynomial is fit to the waypoints. I think this description does a great job of explaining the details of the Udacity MPC, and I'm pasting here one of the figures from that source: The x-axis is pointed to the direction in which the car is facing, and the y-axis is perpendicular to that, going out from the left side of the car. Also, the center of the coordinate system is at the center of mass of the car.

But it doesn't quite explain what I mean by "this is no longer a function, and the polynomial fitting fails". It was easiest for me to draw the case I was referring to on a piece of paper:

In the hand-drawn picture the polynomial fit is depicted in red, and the upcoming waypoints form a U-turn. In order to fit the polynomial I have to exclude the last three waypoints, because the curve C(x, y) that goes through all the waypoints is no longer a function (F: X --> Y) in the sense that: for each value in X there no longer is exactly one value in Y.

Currently, I'm just excluding those waypoints. Maybe you've experienced a problem like this, but I'm starting to suspect it's inherently a limitation of this approach to MPC.

MPCC does not suffer from this limitation and it's the best approach to autonomous racing I've encountered so far. Alas, I'm not using MATLAB, and since I don't know Octave it would take considerable amount of time for me to adapt it.

So I'm happy to hear that you are working on a C++ version. I could also contribute a Python version (if you would be willing to accept it); I've already did that with the Udacity implementation and tested out in the CARLA environment.

I don't want to impose with the C++ implementation of MPCC, but if you'll have it in a "almost works" phase, I can test it out and perhaps provide you with some feedback (again, if you'd want to accept it).

[alexliniger]

Instead of having a function F:X-->Y the general idea (also used in the MPCC) is to use a path which is parametrized by arc-length. Thus you have two functions F_x:s-->X and F_y:s-->Y, where s is the arc length, the heading of the path is then given by atan2(dF_y/ds,dF_x/ds). In the readme I use Theta as the arc length but s is commonly used in literature. This solves the "not a function problem" but may need quite some changes of the Udacity MPC code.

A python version could be interesting, I actually thought about a Julia version since they have a very nice optimization interface I like. But I think I will push the C++ implementation as it is practically the most important. Also for future projects I have. If you want to provide a Python version I am happy to include it in the repo and give you feedback. If it is getting ready, feedback for the C++ version is also welcome.

[PranjalBiswas]

So, is it that polynomial fitting then needs to be performed for both F_x and F_y separately which are functions of s (arc length)?

[alexliniger]

Yes, but this then also requires that you need to the arc-length s_k to recover X and Y at time step k. This is the reason the MPCC has the theta state, thus, if you want to solve the "not a function" problem like this you basically end up with the MPCC or a similar formulation. (I only noticed this after I wrote the answer.)

[PranjalBiswas]

Ok. I might sound a bit naive here, but if I parametrize (I still need to figure out how exactly) my reference path F: X->Y by arc-length (s or theta) as F_x:s-->X and F_y:s-->Y, then for a given "s" I can evaluate the corresponding X and Y anyway. So correct me if I am wrong, the path following approach applied to a contouring problem (MPCC) helps us in a way to associate the arc length (s) to a specific instant of time (k) using velocity (v) as the state, which in turn relates to the progress along the path (F: X->Y)?

[MTDzi]

Yes, but this then also requires that you need to the arc-length s_k to recover X and Y at time step k. This is the reason the MPCC has the theta state, thus, if you want to solve the "not a function" problem like this you basically end up with the MPCC or a similar formulation. (I only noticed this after I wrote the answer.)

OK, I think I get it.

I was thinking about changing the code and parametrizing the curve, but that lead me to the conclusion that the way it's handled in MPCC (in which the car needs to fit within a corridor specified as constraints) is more natural.

@PranjaLBiswas27

...then for a given "s" I can evaluate the corresponding X and Y anyway.

Yes, that's right.

So correct me if I am wrong, the path following approach applied to a contouring problem (MPCC) helps us in a way to associate the arc length (s) to a specific instant of time (k) using velocity (v) as the state, which in turn relates to the progress along the path (F: X->Y)?

This is a jump from the previous sentence, but considering Alex'es comment, I would say that the formulation of the problem in MPCC (in terms of s and v) automatically deals with "not a function" problem because it inherently treats the path as a parametrized curve.

[PranjalBiswas]

Well as @alexliniger said, we need to recover X and Y at time step k using arc_length s_k. So my doubt is how do we get a relation between arc length and time. Cause when i say "...then for a given "s" I can evaluate the corresponding X and Y anyway." I am trying to say that X=f(s) and Y=f(s), and hence I can evaluate X and Y for any given s. This again leaves me a geometric path F: X->Y. So there is no relation with time here.

Hence, does it has to do something with how we parametrize the geometric path (F: X->Y), which makes arc_length (s) in itself a function of time (t)? Or it is a consequence of the MPCC approach?

[alexliniger]

In the MPCC formulation, a virtual state theta is introduced (as said theta and s is the same), this state is there to approximate the arc length at each time step in the horizon. The lag error is used to bind theta to the true arc length based on the geometry of the path. Alternatively, there is a very similar path following method which does not rely on the additional state theta but transforms the system in a local curvilinear coordinate system (w.r.t. the path). By doing so the states become the current arc length, perpendicular distance to the path, relative heading to the path and the velocities. Both approaches have their pros and cons but to explain them I would need to go into the math.

In summary, when you use an arc length parametrized path you need a state which corresponds to the current arc length. This gives you the arc length with temporal information.

A path parametrization of the form F:X->Y is very restrictive, as discussed in with the issue MTDzi had, and in my opinion, as long as you use this parametrization you can not solve the "not a function" issue

[PranjalBiswas]

Well thanks, I will look into the curvilinear coordinate approach to get an insight into that.

Ok, I see. So if I understand correctly, then the temporal relation of the actual arc length comes as a consequence of introduction of virtual state (theta) which is an approximation of the actual arc length.

[alexliniger]

I think we can close this issue if there are some further questions you can reopen it or start a new issue

[MTDzi]

Agreed.