A vacuum robot stands at the pose a = (xa, ya, ta) and must end exactly at the docking pose b = (xb, yb, tb): positions in metres (x to the right, y up), headings in radians counter-clockwise from the +x axis. It can only turn on the spot and drive straight forwards, so it plans three moves:
- turn on the spot by
alpharadians, to face the dock's position; - drive
d >= 0metres straight to that position; - turn on the spot by
betaradians, to match the dock's headingtb.
Both turns go the shorter way round, so alpha and beta lie in (-\pi, \pi]; a half turn is written as +\pi. Positive turns are counter-clockwise. If the two positions are exactly the same, the robot does not drive: alpha = 0, d = 0, and beta is the whole change of heading.
Write turn_drive_turn(a, b) that returns the tuple (alpha, d, beta).
Examples
Input: a = (0, 0, 0), b = (0, 2, 0)
Output: (1.5707963267948966, 2.0, -1.5707963267948966)
Explanation: face up (a quarter turn left), drive 2 m, turn back to face +x.
Input: a = (1, 1, 1.5707963267948966), b = (1, 1, -1.5707963267948966)
Output: (0.0, 0.0, 3.141592653589793)
Explanation: already at the dock's position; only a half turn is needed.
Input: a = (2, -1, 3.0), b = (-1, 3, -2.5)
Output: (-0.7857025644118192, 5.0, 1.5688878715914054)
Explanation: the leg (-3, 4) is 5 m long and points at heading 2.2143. From 3.0 that is a
turn of -0.7857; from 2.2143 to -2.5 is -4.7143, the long way round, so the robot turns
+1.5689 instead.
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Plan the turn, the straight drive and the final turn that take a robot from one pose to another
- Measure both turns against the heading of the straight leg
- Handle the special case where only the heading has to change