Let XY be a standard (right-handed) Cartesian coordinate system fixed with respect to the field surface.
The center-of-geometry[1] of a 4-wheel crab-drive[2] robot is sitting at coordinates (X0,Y0) and its orientation (longitudinal axis) is “alpha” radians clockwise from the +Y axis.
All 4 wheels are turned to a steering angle “beta” radians clockwise from the longitudal axis of the robot.
The trackwidth of the robot is “W” and the wheelbase is “L”.
Find the new steering angle “sigma” necessary to drive the robot in a straight line so that its center-of-geometry is located at point (X1,Y1).
Notes:
[1] “center of geometry” in this context means the center of the rectangle formed by the locations of the 4 wheels
[2] “crab drive” in this context means all 4 wheels are steered to the same angle and are driven at the same speed
Given I tried solving this in between lectures, I am not certain of my solution.
First, let us define an angle Phi that is the angle in radians from the +y axis to the vector of intended motion <x1-x0,y1-y0>. Phi is equal to the sum of the two angles Alpha (The angle from +y to the front of the robot) and Sigma (The angle from the front of the robot to the direction of the wheels).
Since both angles are measured clockwise from their respective reference lines, the angles are added together.
Constructing a right triangle with <x1-x0,y1-y0> as the hypotenuse and the legs parallel to the Cartesian axes, we find the Angle from +y to the vector of intended motion to be arctan((x1-x0)/(y1-y0)). This may look odd at first, but the use of the change in x and y terms in the arctangent function is due to measuring the angle with respect to the y axis instead of the x axis.
Additionally, the wheels must be turned Sigma - Beta from their initial configuration. (not that the problem asked for it.)
I disregarded the width and length of the wheelbase because the initial and final coordinates were already based on the center of geometry, meaning I could treat this as an omnidirectional unicycle bot (I challenge someone to make it).
This is my solution for the posted problem; I trust it has errors, but I am not certain where they are, so any critiques or suggestions are welcome.
I have included a somewhat mediocre drawing of what this looks like.
Actually, since the bot as described does not rotate, any point on the bot could have been used (as long as it was the same point for both the initial and final coords).
This is my solution for the posted problem; I trust it has errors, but I am not certain where they are, so any critiques or suggestions are welcome.
Nice work. Reps to you. You’re 95% there…
The formula you gave gives the “correct” angle if y1>y0, but gives the “reverse” angle (180 degrees off) if y1<y0. If y1=y0, it crashes.
For example:
if x0=0, y0=0, x1=10, y1=-1, and alpha=90 degrees, the formula gives sigma= -174.3 degrees instead of +5.7 degrees.
if x0=0, y0=0, x1=10, y1=0, and alpha=60 degrees, the formula crashes instead of sigma=30 degrees.
There’s a simple way fix the formula that does not involve any conditional logic in your code.
Additionally, the wheels must be turned Sigma - Beta from their initial configuration
Let’s call that angle “delta”. Is there a simple formula (not involving conditional logic) that will compute the shortest value for delta? (For example if delta is 270 degrees the formula should give -90 degrees).
From my reasoning, the code only crashes due to a division by zero in the arctangent function. This could be fixed by using arcsin ((x1-x0)/sqrt ((x1-x0)^2+(y1-y0)^2)). This crashes if the inital and final positions are the same, but the problem is meaningless if you are already at yor final position.
Regarding the smallest Delta, either a conditional check on the absolute value of Delta being larger than 180 degrees or running the delta through arcsin(sin(Delta)) could do the trick. I know the second suggestion is implemetation dependent and not the ideal method but it is a method that I think will work in most cases.
That actually made the problem worse. It gives the wrong answer if y1<y0.
For example: If x0=0, y0=0, x1=10, y1=-1, and alpha=90 degrees, that formula gives sigma= -5.7 degrees instead of the correct +5.7 degrees.
There’s a simple alternative to arctan(dx/dy) that gives the correct angle for all values of dx and dy except when they are both zero.
Regarding the smallest Delta, either a conditional check on the absolute value of Delta being larger than 180 degrees or running the delta through arcsin(sin(Delta)) could do the trick. I know the second suggestion is implemetation dependent and not the ideal method but it is a method that I think will work in most cases.
There’s a calculation that produces the smallest angle without conditional logic or trig functions.
The simple alternative to atan(dy/dx) wouldn’t happen to be atan2(dy, dx), would it? I am unsure if the problem permits us to use computer functions that happen to wrap up the conditonal logic nicely.
Edit: I just saw the thread with StangPS and the video. I see you did live up to your comments about disecting the math. Well played. I feel foolish for reinventing the wheel now.
Yes it would. atan and atan2 have different behaviors.
atan’s range is limited to -90 < atan < +90. So if the problem involves an angle like 95.7 degrees (as in the previously posted example), atan cannot return that value.
atan2’s range is -180 <= atan2 <= +180 and it handles the quadrantal angles without crashing. Also, in many implementations (Octave is an example) atan2 handles the arguments (0,0) and returns 0.
You can wrap conditional logic around atan in your own code to produce your own version of atan2, but it’s better to use the library version if it’s available. Pages 152 thru 158 of P J Plauger’s classic 1992 book The Standard C Library shows a reference implementation of atan and atan2.
I don’t think you re-invented the wheel. Your diagram and accompanying explanation stated the simplifying assumptions explicitly, used a consistent reference coordinate system, and were very clear and easy to understand.
