wrap data

[DreamonZhu]

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, https://github.com/alexliniger/MPCC/blob/3ec7805e7a176c57250c1a414a6d2b871e472d74/C%2B%2B/Spline/arc_length_spline.cpp#L181 https://github.com/alexliniger/MPCC/blob/3ec7805e7a176c57250c1a414a6d2b871e472d74/C%2B%2B/Spline/cubic_spline.cpp#L157 I don't quite understand the purpose of these two lines. Did they just make x_max = 0 ? Then, https://github.com/alexliniger/MPCC/blob/3ec7805e7a176c57250c1a414a6d2b871e472d74/C%2B%2B/MPC/mpc.cpp#L167 when will this condition get satisfied? https://github.com/alexliniger/MPCC/blob/3ec7805e7a176c57250c1a414a6d2b871e472d74/C%2B%2B/types.h#L55 Similarly, when will s > L or s < 0? Thirdly, https://github.com/alexliniger/MPCC/blob/3ec7805e7a176c57250c1a414a6d2b871e472d74/C%2B%2B/Cost/cost.cpp#L187 what is the purpose to do this way with theta_ref instead of just using it?

[alexliniger]

This happens if you write code over a certain time. There are a lot of ways to wrap signals and I used different approaches.

The first and the third approach are simple wrapping approaches that work even if the difference is more than one period. The first works for signals which wrap between 0 and x_max, the second is for angles.

This line https://github.com/alexliniger/MPCC/blob/3ec7805e7a176c57250c1a414a6d2b871e472d74/C%2B%2B/MPC/mpc.cpp#L167 and actually also that one https://github.com/alexliniger/MPCC/blob/3ec7805e7a176c57250c1a414a6d2b871e472d74/C%2B%2B/MPC/mpc.cpp#L158 Is an unwrapping approach, the issue is that the initial guess you give the optimizer has to be continuous, so for example if you are currently at the end of the track, the initial guess is should not wrap when it crosses the start finish line but it should keep on increasing, similar for the angle. If you have a jump in the initial guess, this can be problematic. However, as you now have values that are not wrapped you need all the other functions to take care of that. Maybe I overdid it at some points but I wasted to a lot of time finding bugs related to this issue.

I hope this helps a bit. Best, Alex

[DreamonZhu]

OK, I got you. But I thought (initial_guess_[i].xk.phi - initial_guess_[i-1].xk.phi) < M_PI will always satisfy when car's yaw rate is not too large during normal driving in one circle except crossing the start finish line. Then why should I do the operation initial_guess_[i].xk.phi += 2*PI in those situations.

[alexliniger]

The issue is that the heading of x0 is always within the range [-pi, pi]. Thus whenever x0 gets wrapped, this statement becomes active.

Best, Alex

[DreamonZhu]

Sorry to bother you again. Why isn't the condition (initial_guess_[i].xk.phi - initial_guess_[i-1].xk.phi) < -M_PI ? This condition describes the situation when the heading angle of the car become a number in [0, -pi] from that of [0, pi]. And because of that, the initial guess is not continuous, then we just make initial_guess_[i].xk.phi += 2*pi to make initial guess continuous. So my question is that why the right-hand side of inequality (initial_guess_[i].xk.phi - initial_guess_[i-1].xk.phi) < M_PI isn't -M_PI ?

[alexliniger]

you are right i fixed it in the code