Is the straight line safe?

Safe ends, unsafe middle

Planners connect configurations with straight lines in joint space:

$$q(s) = q_a + s\,(q_b - q_a), \qquad 0 \le s \le 1$$

Both ends can be free while the middle sweeps straight through the wall. A motion (an edge of the planner's graph) is only valid if every configuration on it is free. There are infinitely many of those, so you check a finite sample of them.

How many samples?

Space the samples so that no joint turns more than res radians between two checks. The joint that changes most sets the count, which is the max-norm of the difference:

$$n = \left\lceil \frac{\max_i \lvert q_{b,i} - q_{a,i} \rvert}{\text{res}} \right\rceil, \qquad s_k = \frac{k}{n}, \quad k = 0, 1, \dots, n$$

Why 0.02 rad? Between two samples, every joint can turn by up to 0.02 rad at once. The grasp point is at most $L_1 + L_2 + L_3 = 0.67$ m from the base axis, so the base swings it sideways by at most $0.67 \times 0.02 \approx 1.3$ cm. The shoulder, elbow and wrist move it within the arm's own plane, by at most $(0.67 + 0.37 + 0.12) \times 0.02 \approx 2.3$ cm between them. Those two directions are at right angles, so the gripper jumps at most $\sqrt{1.3^2 + 2.3^2} \approx 2.7$ cm from one sample to the next. The wall is only 2 cm thick, but in_collision counts anything within 5 mm as a hit, so to the samples it's 3 cm thick and none of them can jump over it. Coarser steps can.

MoveIt and OMPL work the same way. Their "longest valid segment" setting plays the same role as res.

Your task

Implement segment_free(qa, qb, res=0.02). It returns True only if every sampled configuration from qa to qb is free.

The starter draws the gripper's route for the straight move from A (above the balls) to B (above the cup), green where it's free and red where it collides. It only drives the arm if segment_free says the line is safe. Right now the function checks just the two ends, so run it and watch the arm hit the wall.

The grader tries your function on 30 motions. Half are safe. The other half clip the wall or the pillar somewhere in the middle, and most of them only for a moment.