Inverse kinematics & straight-line motion

Running FK backwards

Tasks are usually stated in the world ("put the gripper here"), but motors only understand joint angles. Inverse kinematics (IK) solves for the configuration that places the gripper at a target $\mathbf{p}$:

$$q = \operatorname{IK}(\mathbf{p})$$

For now you get a solver: arm.ik(p) returns joint angles that put the grasp point at p with the gripper pointing straight down. You'll write your own in the bonus step.

Joint space vs. Cartesian space

Interpolating the joint angles from $q_A$ to $q_B$ is simple, and it's what move_joints does. The gripper's path, however, comes out curved, because every joint turns at once. That's fine in open air. Near objects you want the gripper to travel in a predictable straight line.

Motion planners like MoveIt handle this with a Cartesian path: they sample points along the line and solve IK at every step.

$$\mathbf{p}(t) = \mathbf{p}_A + s(t)\,(\mathbf{p}_B - \mathbf{p}_A), \qquad q(t) = \operatorname{IK}(\mathbf{p}(t))$$

Timing: minimum jerk

If $s$ went linearly from 0 to 1, the gripper would start and stop instantly. That means infinite acceleration, which makes a real arm jolt. The classic fix is the minimum-jerk profile, with $\tau = t/T$:

$$s(\tau) = 10\tau^3 - 15\tau^4 + 6\tau^5$$

It starts and ends with zero velocity and zero acceleration.

Your task

The three yellow markers are waypoints (world.targets()). Implement min_jerk and move_linear, then trace the waypoints in order. Your path must stay within 2 cm of each straight segment between them.

The starter code uses move_joints. Run it first and look at the orange trail: the path bows away from the straight lines.

Send new joint targets every 10 ms, which is a typical control rate.