Trailer: terminal conditions not met

[timmercy]

fixedT: sometimes the end position is reached but the terminal condition on the trailer angle is not met...

freeT: sometimes the terminal conditions are never met, while the vehicle has arrived at correct end position. The reason is that when raising the amount of knots the freedom towards the end of the problem will greatly raise. This is because all knots are squeezed together between the current position and the goal position. The method can't always handle this freedom well. Sometimes the inputs get a lot of peaks in this last stage of the receding horizon problem.

These problems are linked to issue #41. It would be better to use a fixedT problem with constraints which guarantee that you reach the goal position.