Fighting Singularities in 6-DOF Manipulators 🦾
That moment when your Inverse Kinematics solver explodes. Implementing Damped Least Squares (DLS) for VR Teleoperation.
Update: Dec 16, 2025
Successfully presented the VR Interface Prototype today! The DLS solver handled the live demo stresses perfectly. Next stop: Haptic feedback integration.
The Problem: Gimbal Lock's Evil Brother
During VR teleoperation testing with the Kinova MICO2, I noticed that when the arm reached full extension, the virtual model would jitter violently or snap to random configurations.
Diagnosis: Jacobian Singularity. The determinant of the Jacobian matrix approached zero, causing the inverse calculated velocities to approach infinity. In simple terms: the math broke because the arm didn't know which way was "up".
The Fix: Damped Least Squares
Instead of using the standard Moore-Penrose pseudoinverse J†, I switched to the Damped
Least Squares (Levenberg-Marquardt) method.
Basically, we add a "damping factor" λ to the equation. It sacrifices a tiny bit of
accuracy near singularities to ensure the velocities remain finite.
$/ solver_utils.py
def damped_least_squares(J, lambda_val=0.1):
# J_dls = J.T * (J * J.T + lambda^2 * I)^-1
inv_term = np.linalg.inv(J @ J.T + lambda_val**2 * np.eye(J.shape[0]))
return J.T @ inv_term
Result
The movement is now buttery smooth even when I try to break it by fully extending the arm in VR. The robot slows down slightly near singular configurations (as it should) but never loses stability.