Just to give you an idea on what I’m dealing with and what the cost term actually is, I am trying to minimize the squared contouring and lag errors while covering the maximum possible distance on my reference track.
I am currently testing the reference tracking capability of my OCP where the car simply follows the track, i.e. the maximum possible distance term hasn’t been included yet.
The OCP seems to be working fine till a particular point on my test track where it randomly returns the error: “QP solver returned error status 3”.
I think this means my problem has become infeasible and I violate my constraints. So, I tried evaluating my constraint values at every point on my trajectory and it is shown to be well within the limits I imposed.
Even after the solver returns error status 3.
I am a bit confused by this behavior and have tried all means of solving it. If any of you might have an inkling as to why this could take place, and can help me out, it would be much appreciated.
I am happy to provide more information if necessary.
Hello @kaethe Thank you for your reply. I looked into this issue and I seem to have found a possible source for this error. As I have mentioned before, I am trying to implement time optimal MPC on an F1/10 car system. For simulation purposes I’m using odometry to measure my car states (pose). And this leads to my state space being discontinuous since my car’s orientation is in the interval [-pi, pi).
I wanted to ask if ACADOS is capable of handling discontinuous state spaces. My intuition says yes as a lot of robotics applications have discontinuous state spaces (Rotational states) but I am not very sure. Any help on how to handle this issue would be greatly appreciated.
I am currently using a simple kinematic bicycle model for predictions and my states are the X and Y positions of the center of gravity of my car and the orientation itself. These are all well defined in this set. I am unsure as to why the discontinuity causes an error, I thought it might be just be ACADOS’ inability to handle the discontinuity itself.
While traversing the track, my car’s orientation changes continuously and at a point the angle is less than -pi but my odometry returns a value that is less than pi instead which is, technically speaking, the same angle, measured anti-clockwise instead of clockwise. This is the point when the solver returns this error message, it has happened too many times at the exact same point for it to be a coincidence.
I am still looking into possible causes for this problem and checking if I missed something. I have some theories as to why this might be happening but have not found any definite proof for these theories yet.
Could you just change your odometry system or add a layer, such that the initial angle is as close as possible to the previous one?
Otherwise, you could change your solver initialization by shifting all angles by \pm\pi if this happens.
Hello @FreyJo sorry for the late reply. I tried using angle unwrapping so that my angle values would remain continuous but this results in the solver becoming unstable for large running times. i.e. the solution does work temporarily but if I were to keep running the car it eventually crashes due to the wrong input. It is possible that because of the unwrapping the integration error accumulates causing this problem, but I am still not sure why this is happening.
