5.4. Jacobian Inverse Kinematics

Jacobian Inverse Kinematics

Jacobian-based inverse kinematics uses the Jacobian to iteratively solve the inverse kinematics problem numerically. Rather than finding a closed-form solution, the algorithm linearizes the forward kinematic map at each step and takes a small step in joint space toward the desired pose.

Given a desired pose $g_D\in SE(3)$ and a current configuration ${\Theta }_t$, the spatial velocity needed to correct the pose error is extracted using the matrix logarithm:
${\hat{V}}_D=\log (g_D{g}_{st}({\Theta }_t{)}^{-1})\in \mathfrak{se}(3),$
which gives the twist that would carry the current end-effector pose to the desired pose in one exponential step. The joint update is then
${\Theta }_{t+1}={\Theta }_t+\alpha J^s({\Theta }_t{)}^{†}{V}_D,$
where $J^{s†}$ is the pseudoinverse of the spatial Jacobian and $\alpha \in (0,1]$ is a step size. The algorithm iterates until $\Vert V_D\Vert <ϵ$.

Near singularities, the pseudoinverse amplifies noise and produces enormous joint velocities. Damped least squares (Levenberg–Marquardt) regularizes this by solving
${\Theta }_{t+1}={\Theta }_t+\alpha J^T(J{J}^T+{\lambda }^{2}I{)}^{-1}{V}_D,$
which is the solution to the regularized problem ${\min }_{\dot{\Theta }}\Vert J\dot{\Theta }-V_D{\Vert }^2+{\lambda }^2\Vert \dot{\Theta }{\Vert }^2$. The damping parameter $\lambda >0$ trades off tracking accuracy for bounded joint velocities.

For redundant manipulators ($n>6$), the pseudoinverse provides a minimum-norm joint velocity, and the remaining null-space freedom can be used for secondary objectives:
$\dot{\Theta }=J^{†}{V}_D+(I-J^{†}J)w,$
where $w\in {\mathbb{R}}^n$ is an arbitrary vector and $(I-J^{†}J)$ projects it into $\mathcal{N}(J)$.

Local Method Paden–Kahan subproblems), analytic methods are preferred when available.

Jacobian IK is a local, iterative method. It can converge to a local minimum (especially when the target is unreachable), may oscillate near singularities, and does not enumerate all solutions. For manipulators with closed-form IK (e.g., via

Intuition

The Jacobian IK algorithm is essentially Newton’s method applied to the equation $g_{st}(\Theta )=g_D$ on the Lie group $SE(3)$. At each iteration, the “error” between the current and desired pose is measured not by subtracting matrices (which is meaningless for elements of $SE(3)$) but by computing the relative transform $g_D{g}_{st}^{-1}$ and taking its logarithm. The logarithm maps the error from $SE(3)$ back to the Lie algebra $\mathfrak{se}(3)$, where it becomes a twist vector that can be linearly related to joint velocities via the Jacobian. This is the natural generalization of Newton’s method to manifold-valued functions.

Damped least squares can be understood geometrically: instead of inverting the Jacobian exactly (which blows up when singular values are small), it “shrinks” the inverse of each singular value from $1/{\sigma }_i$ to ${\sigma }_i/({\sigma }_i^2+{\lambda }^2)$. Large singular values are barely affected, but small ones are smoothly attenuated, preventing the explosion. The cost is reduced accuracy in the singular directions—the end-effector may not reach the exact target along those directions, but the joint velocities remain bounded.

The null-space projection for redundant arms allows the manipulator to “use its extra joints” for a secondary task (like maximizing manipulability, avoiding joint limits, or dodging obstacles) without affecting the primary end-effector tracking. The projection $(I-J^{†}J)$ kills any component of $w$ that would change the end-effector velocity, keeping only the self-motion component.

Properties

The Matrix Logarithm on $SE(3)$

(Definition) For $g\in SE(3)$ with $g=\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]$, the logarithm $\hat{\xi }\theta =\log (g)\in \mathfrak{se}(3)$ recovers the twist $\xi$ and angle $\theta$ such that $e^{\hat{\xi }\theta }=g$. The rotational part uses the $SO(3)$ log map to find $(\omega ,\theta )$ from $R$. The translational part solves $p=A(\theta )v$ where

$A(\theta )=\frac{\sin \theta }{\theta }I+\left(1-\frac{\sin \theta }{\theta }\right)\omega {\omega }^T+\frac{1-\cos \theta }{\theta }\hat{\omega },$
giving $v=A(\theta {)}^{-1}p$.

(Error Twist) The twist $V_D=(\log (g_D{g}_{st}^{-1}){)}^{\vee }\in {\mathbb{R}}^6$ is the error expressed in the spatial frame. When $g_{st}=g_D$, $V_D=0$. The body-frame variant uses $(\log (g_{st}^{-1}{g}_D){)}^{\vee }$ and the body Jacobian.

Pseudoinverse Methods

(Moore–Penrose Pseudoinverse) For $J\in {\mathbb{R}}^{6\times n}$ with $n\ge 6$ (redundant or square), the right pseudoinverse $J^{†}=J^T(J{J}^T{)}^{-1}$ gives the minimum-norm solution $\dot{\Theta }=J^{†}V$ to the underdetermined system $J\dot{\Theta }=V$. For $n<6$, the left pseudoinverse $J^{†}=(J^{T}J{)}^{-1}{J}^T$ gives the least-squares solution.

(Via SVD) If $J=U\Sigma V^T$, then $J^{†}=V{\Sigma }^{†}{U}^T$ where ${\Sigma }^{†}$ replaces each nonzero ${\sigma }_i$ with $1/{\sigma }_i$. This makes the singular-value amplification explicit: near a singularity, ${\sigma }_i\to 0$ and $1/{\sigma }_i\to \infty$.

Damped Least Squares

(Formulation) Solve ${\min }_{\dot{\Theta }}\Vert J\dot{\Theta }-V_D{\Vert }^2+{\lambda }^2\Vert \dot{\Theta }{\Vert }^2$. The solution is

$\dot{\Theta }=J^T(J{J}^T+{\lambda }^{2}I{)}^{-1}{V}_D.$
Equivalently, via SVD: each singular value contribution ${\sigma }_i/({\sigma }_i^2+{\lambda }^2)$ replaces the pseudoinverse’s $1/{\sigma }_i$.

(Damping Parameter) $\lambda =0$ recovers the pseudoinverse. Larger $\lambda$ gives smoother, bounded joint velocities at the cost of tracking accuracy. In practice, $\lambda$ can be adapted: small when far from singularities (${\sigma }_{\min }$ large), increased as ${\sigma }_{\min }\to 0$.

(Convergence) Damped least squares converges linearly. The algorithm does not guarantee reaching the exact target at a singularity—the damping term prevents the last bit of correction in the singular direction. This is acceptable because the alternative (infinite joint velocity) is physically impossible.

Redundant Manipulators and Null-Space Optimization

(Null-Space Projection) For $n>6$ with $\mathrm{rank}(J)=6$, the general solution to $J\dot{\Theta }=V$ is

$\dot{\Theta }=J^{†}V+(I-J^{†}J)w,$
where $w\in {\mathbb{R}}^n$ is arbitrary. The matrix $N=I-J^{†}J$ is the orthogonal projector onto $\mathcal{N}(J)$: it satisfies $N^2=N$, $N^T=N$, and $JN=0$.

(Secondary Objectives) Common choices for $w$ include:

• Manipulability maximization: $w=k_0{\nabla }_{\Theta }{\mu }_3(\Theta )$, pushing the configuration toward higher manipulability.
• Joint-limit avoidance: $w=-k_0{\nabla }_{\Theta }H(\Theta )$ where $H$ penalizes proximity to joint limits.
• Obstacle avoidance: $w$ chosen to increase distance from obstacles.
  Since $Nw\in \mathcal{N}(J)$, the secondary objective does not affect the end-effector velocity.

(7-DoF Example) A 7-DoF arm (e.g., KUKA iiwa) has a 1D null space at non-singular configurations. The self-motion is an “elbow swivel”—the elbow traces a circle while the wrist position and orientation remain fixed. This extra DoF allows the arm to reach around obstacles or avoid joint limits.

Step Size Selection

(Constant Step Size) $\alpha \in (0,1]$. Smaller $\alpha$ increases stability but slows convergence. $\alpha =1$ corresponds to a full Newton step.

(Adaptive Step Size) When $\Vert V_D\Vert$ is large, a small $\alpha$ prevents overshooting; when $\Vert V_D\Vert$ is small, $\alpha$ can be increased for faster convergence. A common heuristic is $\alpha =\min (1,{\alpha }_{\max }/\Vert V_D\Vert )$.

Examples

Jacobian IK Algorithm (Pseudocode)

Full algorithm

Input: g_D (desired pose), Theta_0 (initial config),
      alpha (step size), epsilon (tolerance), max_iter

Theta = Theta_0
for t = 1 to max_iter:
   g_current = FK(Theta)
   error_g = g_D @ inv(g_current)
   V_D = vee(logm(error_g))        # 6-vector twist error

   if ||V_D|| < epsilon:
       return Theta                  # converged

   J = spatial_jacobian(Theta)

   # Option A: Pseudoinverse (away from singularities)
   dTheta = alpha * pinv(J) @ V_D

   # Option B: Damped least squares (near singularities)
   # dTheta = alpha * J.T @ inv(J @ J.T + lam^2 * I) @ V_D

   # Option C: Redundant with null-space (n > 6)
   # dTheta = alpha * pinv(J) @ V_D + (I - pinv(J) @ J) @ w

   Theta = Theta + dTheta

return Theta  # may not have converged

Damped Least Squares Near Singularity

A 2R arm at ${\theta }_2=0.01$ (near full extension) with desired radial velocity $V_D=\left[\begin{array}{c}1\\ 0\end{array}\right]$:

Comparing pseudoinverse vs. damped $J$: ${\sigma }_1\approx 2$, ${\sigma }_2\approx 0.01$.

Singular values of

Pseudoinverse: $\dot{\theta }\approx 1/{\sigma }_2=100$ in the singular direction—physically unrealizable.

Damped ($\lambda =0.1$): contribution ${\sigma }_2/({\sigma }_2^2+{\lambda }^2)\approx 0.01/0.0101\approx 0.99$—bounded and smooth. The end-effector won’t perfectly track the radial command, but joint velocities stay reasonable.