Formulation of translational vector of the vehicle

[hect1995]

When setting the constraints for the collision avoidance -g'*mu + (A*t - b)*lambda > 0 you set the translation t(x) on x as x[1,i]+cos(x[3,i])*offset1 , translation on y as x[2,i]+sin(x[3,i])*offset1. In the case of a 2 ergo vehicle you declare the t vector as [x[1,i]-cos(x[4,i])*offset2 , x[2,i]-sin(x[4,i])*offset2]. I do not understand how did you get into these results, mainly because I do not understand how the terms multiplying the offset appear

[alexliniger]

This is somehow a hack as offset2 is in reality negative, it points in the other direction than offset1, but in my code it is positive, thus I added the minus when I compute the center of the second ego vehicle.