The Jacobian and velocity

Position kinematics tells you where the hand is. The Jacobian tells you how fast it moves when the joints move, and, turned around, how fast each joint must turn to make the hand move the way you want. It is also the cleanest way to see why arms misbehave near full stretch. If you want a hand that follows a moving target, draws a straight line or pushes steadily against a surface, you need it.

What the Jacobian is

The tip position is a function of the joint angles: p = f(q), with q = (t1, t2) and p = (x, y). Differentiate it with the chain rule:

dp/dt = J(q) * dq/dt, written v = J * qdot

J is the Jacobian: a matrix of partial derivatives, J[i][j] = d(p_i)/d(q_j). It is a local linear map from joint velocities (rad/s) to tip velocity (mm/s). It depends on the pose, so it must be recomputed whenever the arm moves.

The Jacobian of the two-link arm

Start from the forward equations and differentiate each with respect to t1 and t2. Write s1 = sin(t1), c1 = cos(t1), s12 = sin(t1 + t2), c12 = cos(t1 + t2):

x = L1*c1 + L2*c12 y = L1*s1 + L2*s12

dx/dt1 = -L1*s1 - L2*s12, dx/dt2 = -L2*s12 dy/dt1 = L1*c1 + L2*c12, dy/dt2 = L2*c12

So

J = [[-L1*s1 - L2*s12, -L2*s12], [L1*c1 + L2*c12, L2*c12]]

Notice the first column is (-y, x): rotating the shoulder swings the tip around the base, perpendicular to the line to the tip, with speed proportional to the distance r. The second column is the same idea around the elbow, with distance L2.

A worked example

Use L1 = 100 mm, L2 = 80 mm, t1 = 30 degrees, t2 = 45 degrees from lesson 2, where the tip is at (107.31, 127.27) mm. With s1 = 0.5, c1 = 0.866, s12 = sin 75 = 0.9659, c12 = cos 75 = 0.2588:

J = [[-127.27, -77.27], [107.31, 20.71]] in mm/rad.

Let the joints turn at t1dot = 0.2 rad/s and t2dot = 0.1 rad/s. The tip velocity is:

vx = -127.27*0.2 + (-77.27)*0.1 = -25.45 - 7.73 = -33.18 mm/s vy = 107.31*0.2 + 20.71*0.1 = 21.46 + 2.07 = 23.53 mm/s

The tip speed is sqrt(33.18^2 + 23.53^2) = 40.7 mm/s, heading up and to the left.

Now the useful direction. Suppose you want the tip to rise straight up at v = (0, 20) mm/s. Invert the 2 by 2 matrix. For [[a, b], [c, d]] the inverse is 1/det * [[d, -b], [-c, a]], and here det(J) = (-127.27)*20.71 - (-77.27)*107.31 = 5656.9 mm^2.

J^-1 = 1/5656.9 * [[20.71, 77.27], [-107.31, -127.27]] t1dot = 77.27*20 / 5656.9 = 0.273 rad/s (15.7 degrees/s) t2dot = -127.27*20 / 5656.9 = -0.450 rad/s (-25.8 degrees/s)

The two joints must move in opposite directions and at different speeds. That is Cartesian control: you state a hand velocity and the Jacobian turns it into joint commands.

Singularities: where det(J) = 0

For the two-link arm, the determinant simplifies, using sin(t1 + t2)*cos(t1) - cos(t1 + t2)*sin(t1) = sin(t2):

det(J) = L1*L2*sin(t2)

Check with the example: 100*80*sin 45 = 5656.9. Exactly the number we got. The determinant is zero when sin(t2) = 0, which means t2 = 0 (arm stretched) or t2 = 180 degrees (arm folded). At those poses both columns of J point the same way, so the arm cannot move the tip along the line from base to tip, whatever the joints do. You have lost one degree of freedom, and the inverse J^-1 does not exist.

The trouble starts before you reach it. The inverse contains 1/det, so required joint speeds scale like 1/sin(t2). Ask for 20 mm/s at t2 = 5 degrees and det = 8000*sin 5 = 697 mm^2, so the joints must move about eight times faster than at 45 degrees. A real servo cannot, and the tip drifts off the path.

The pseudo-inverse and resolved-rate control

In practice you rarely have a square, invertible J. A planar arm with 3 links has a 2 by 3 Jacobian: more joints than tip coordinates. A square matrix has an inverse, but a wide one does not. The Moore-Penrose pseudo-inverse fills the gap:

J+ = J^T * (J * J^T)^-1

It returns the joint velocity qdot = J+ * v of smallest norm among all those that produce v. For a square nonsingular J it equals J^-1. Near a singularity J*J^T is nearly singular as well, so a common fix is damped least squares:

J+ = J^T * (J * J^T + lambda^2 * I)^-1

The small constant lambda caps the joint speeds near singularities, at the price of a small tracking error there. This gives resolved-rate control: at each control step, measure the hand error, turn it into a desired velocity, map it to joint velocities, integrate.

import numpy as np

L1, L2 = 0.100, 0.080          # m

def fk(q):
    t1, t2 = q
    return np.array([L1*np.cos(t1) + L2*np.cos(t1 + t2),
                     L1*np.sin(t1) + L2*np.sin(t1 + t2)])

def jacobian(q):
    t1, t2 = q
    s1, c1 = np.sin(t1), np.cos(t1)
    s12, c12 = np.sin(t1 + t2), np.cos(t1 + t2)
    return np.array([[-L1*s1 - L2*s12, -L2*s12],
                     [ L1*c1 + L2*c12,  L2*c12]])

def resolved_rate(q, target, dt=0.02, steps=300, kp=4.0, lam=0.02):
    q = np.array(q, dtype=float)
    for _ in range(steps):
        v = kp * (target - fk(q))                  # desired tip velocity, m/s
        J = jacobian(q)
        qdot = J.T @ np.linalg.solve(J @ J.T + lam**2 * np.eye(2), v)
        q += np.clip(qdot, -3.0, 3.0) * dt         # joint speed limit, rad/s
    return q

q = resolved_rate([np.radians(30), np.radians(45)], np.array([0.05, 0.12]))
print(fk(q))                                       # close to (0.05, 0.12)

The loop is a proportional controller in Cartesian space: the farther the hand is from the target, the faster it moves toward it. With dt = 0.02 s it matches the 50 Hz servo frame. A good habit is to verify jacobian against a finite difference, (fk(q + eps) - fk(q)) / eps, since sign mistakes are common.

On a small microcontroller, the 2 by 2 case can be done by hand with a guard against singularity:

// Solve qdot = J^-1 * v for the two-link arm, with a singularity guard
bool jointRates(float t1, float t2, float vx, float vy, float &w1, float &w2) {
  float s1 = sin(t1), c1 = cos(t1), s12 = sin(t1 + t2), c12 = cos(t1 + t2);
  float a = -L1*s1 - L2*s12, b = -L2*s12;
  float c =  L1*c1 + L2*c12, d =  L2*c12;
  float det = a*d - b*c;                  // equals L1*L2*sin(t2)
  if (fabs(det) < 0.05 * L1 * L2) return false;   // too close to singular
  w1 = ( d*vx - b*vy) / det;
  w2 = (-c*vx + a*vy) / det;
  return true;
}