Angles, atan2 & 2-D rotation

Radians and directions

Angles in code are in radians: a full turn is $2\pi$ rad (360°), so 1 rad is about 57.3°. np.degrees and np.radians convert.

The unit vector at angle $\theta$, measured counter-clockwise from the $x$ axis, is $(\cos\theta, \sin\theta)$. Going the other way, np.arctan2(y, x) gives the angle of the vector $(x, y)$. Plain $\arctan(y/x)$ can't tell $(1, 1)$ from $(-1, -1)$; atan2 looks at both signs, so it is right in every quadrant.

Wrapping

Headings of 179° and −179° are only 2° apart, but subtracting them gives 358°. A rover told to turn 358° spins almost all the way round. Wrapping adds or subtracts whole turns until an angle lies in $(-\pi, \pi]$, which is the short way: 358° becomes −2°.

Rotating in 2-D

Rotating a vector by $\theta$ multiplies it by the rotation matrix

$$R(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix}$$

Its columns are where the $x$ and $y$ axes end up. The same matrix moves readings between frames. The rover's pose is $[x, y, \theta]$: its position and heading in the world. In the rover's own frame, $x$ points straight ahead and $y$ to its left. A beacon seen at range $r$ and bearing $b$ (from straight ahead, counter-clockwise) is at $(r\cos b,\ r\sin b)$ in the rover's frame, and in the world at

$$p_{\text{world}} = R(\theta)\begin{bmatrix} r\cos b \\ r\sin b \end{bmatrix} + \begin{bmatrix} x \\ y \end{bmatrix}$$

Rotate, then shift. Step 5 does the same in 3-D.

Pick & Place, step 1 aims the arm's base at the cup with the same atan2.

Your task

1. Implement rot2(theta) (a 2×2 np.array) and wrap(angle).
2. For each beacon $k$ in world.beacons, in phase(f"beacon {k}"), find its direction from the rover with atan2 and turn to face it the short way: rover.turn(wrap(target - theta)). rover.pose is the rover's true pose $[x, y, \theta]$.
3. Implement beacon_to_world(pose, r, bearing), returning the beacon's world $(x, y)$. The starter compares it with the map after every turn.

The rover starts somewhere new on every run.