Bonus: build your own IK

Opening the black box

arm.ik() and arm.move_to() are locked in this step. You'll write the solver yourself. For an arm like this one there's a neat closed-form solution, built from the same geometry you used for FK.

1. Base angle

Seen from above, the arm has to point at the target:

$$q_0 = \operatorname{atan2}(y,\ x), \qquad r = \sqrt{x^2 + y^2}$$

2. Find the wrist

The gripper points straight down, so the wrist sits exactly $L_3$ above the target. Measured from the shoulder:

$$w_r = r, \qquad w_z = z + L_3 - h_0, \qquad d^2 = w_r^2 + w_z^2$$

3. Elbow: law of cosines

The upper arm, the forearm and the line from shoulder to wrist form a triangle:

$$\cos q_2 = \frac{d^2 - L_1^2 - L_2^2}{2 L_1 L_2}$$

If $|\cos q_2| > 1$ the triangle can't close, which means the target is out of reach. Raise ValueError or return None.

Otherwise $q_2 = \arccos(\cdot)$. Taking the positive root gives the elbow-up solution.

4. Shoulder

The angle to the wrist, minus the angle the forearm adds (both measured from vertical):

$$q_1 = \operatorname{atan2}(w_r,\ w_z) - \operatorname{atan2}(L_2\sin q_2,\ L_1 + L_2\cos q_2)$$

5. Wrist

Point the gripper straight down, which means the tilts must sum to $\pi$:

$$q_3 = \pi - q_1 - q_2$$

Your task

1. Implement inverse_kinematics(p), returning [q0, q1, q2, q3].
2. The starter program already contains a pick-and-place that uses your IK through goto(). Once your IK is right, the red ball goes into the cup.

The grader tests 20 random targets for position and gripper orientation, plus two targets that are out of reach.