Singularities & damped least squares

When the Jacobian loses rank

Stretch the arm out straight and ask the gripper to move further away. No joint speed can do it: every column of $J$ is now perpendicular to the arm, so $J$ has lost a direction. That's a singularity.

Close to one, $J^+$ still returns joint speeds, but huge ones. To gain a millimetre in the almost-impossible direction it spins the joints very fast. Aim at a point out of reach and it asks for well over 100 rad/s.

Measuring it

Manipulability tells you how close you are:

It is proportional to the volume of gripper velocities that unit joint speeds can produce, and it falls to 0 at a singularity.

Damped least squares

Instead of solving $J\,\Delta q = e$ exactly, balance the error against the size of the step:

Far from singularities $\lambda^2$ is tiny next to $JJ^T$, so this is almost $J^+e$. Near one, the damping caps the gain: no direction gets more than $1/(2\lambda)$ times its error. You give up a little accuracy for bounded, safe speeds. The same step, used for IK, is called Levenberg–Marquardt.

Your task

1. Implement dls_step(J, e, lam). Use np.linalg.solve rather than inverting the matrix.
2. In phase("reach") the program drives the gripper toward $(0.80, 0, 0.20)$ for 2 s, commanding at 100 Hz. That point is out of reach: the arm is only 0.67 m long. Replace its np.linalg.pinv step with dls_step(J, e, LAM).

Run the starter first and look at the Plots tab: $w$ falls as the arm straightens, and the joint speeds explode. To pass, keep every commanded joint speed below 5 rad/s and end within 3 cm of full stretch toward the target.