Robot kinematics

Example 1 Reaching out almost to full extension

A two-joint arm with links of 0.40 and 0.30 m has its first joint at 0°. The tool must move outward along x at 0.10 m/s to push a battery into its slot. Compare different second-joint angles (simulated data).

import numpy as np

l1, l2 = 0.40, 0.30          # link lengths m
v_tool = np.array([0.10, 0.0])  # tool must move at 0.10 m/s

def jacobian(t1, t2):
    return np.array([
        [-l1*np.sin(t1) - l2*np.sin(t1+t2), -l2*np.sin(t1+t2)],
        [ l1*np.cos(t1) + l2*np.cos(t1+t2),  l2*np.cos(t1+t2)]])

t1 = np.radians(0)
for deg in [90, 30, 10, 2]:
    J = jacobian(t1, np.radians(deg))
    w = abs(np.linalg.det(J))
    qdot = np.linalg.solve(J, v_tool)
    print(f"θ2={deg:>2}°  manipulability={w:.4f}  "
          f"max joint speed={np.degrees(np.abs(qdot)).max():6.1f} deg/s")

θ2=90°  manipulability=0.1200  max joint speed=  19.1 deg/s
θ2=30°  manipulability=0.0600  max joint speed=  63.0 deg/s
θ2=10°  manipulability=0.0208  max joint speed= 191.2 deg/s
θ2= 2°  manipulability=0.0042  max joint speed= 957.4 deg/s

The straighter the arm (θ2 approaching 0), the lower the manipulability and the higher the joint speed needed, until it exceeds what typical motors can do. Working poses should therefore keep the arm reasonably bent, and the controller should limit joint speeds rather than try to follow impossible commands.

Example 2 A three-joint arm, a reachable target and one too far away

A three-joint arm has links of 0.40, 0.30 and 0.20 m (maximum reach 0.90 m). The first target is the battery slot at (0.50, 0.40) m; the second is a point out of reach at (1.20, 0) m. Compare λ = 0 with λ = 0.2 over 200 iterations (simulated data).

import numpy as np

L = np.array([0.40, 0.30, 0.20])     # three links, maximum reach 0.90 m

def fk(q):
    a = np.cumsum(q)
    return np.array([np.sum(L*np.cos(a)), np.sum(L*np.sin(a))])

def jac(q):
    a = np.cumsum(q)
    J = np.zeros((2, 3))
    for i in range(3):
        J[0, i] = -np.sum(L[i:]*np.sin(a[i:]))
        J[1, i] = np.sum(L[i:]*np.cos(a[i:]))
    return J

def ik(target, lam, iters=200):
    q = np.radians([10.0, 20.0, 20.0])
    max_step = 0.0
    for _ in range(iters):
        e = target - fk(q)
        J = jac(q)
        dq = J.T @ np.linalg.solve(J @ J.T + (lam**2 + 1e-12) * np.eye(2), e)
        max_step = max(max_step, np.degrees(np.abs(dq)).max())
        q = q + dq
    return np.linalg.norm(target - fk(q)), max_step

for lam in [0.0, 0.2]:
    for target in [np.array([0.50, 0.40]), np.array([1.20, 0.0])]:
        err, step = ik(target, lam)
        print(f"lambda={lam}  target {target}  error {err*1000:5.1f} mm  "
              f"largest joint step {step:6.1f} deg")

lambda=0.0  target [0.5 0.4]  error   0.0 mm  largest joint step   76.2 deg
lambda=0.0  target [1.2 0. ]  error 799.9 mm  largest joint step 1480.9 deg
lambda=0.2  target [0.5 0.4]  error   0.0 mm  largest joint step   22.7 deg
lambda=0.2  target [1.2 0. ]  error 300.0 mm  largest joint step   28.4 deg

When the target is reachable, both methods get there, but DLS moves the joints far less per iteration. When the target is out of reach, the pseudoinverse commands joint jumps of more than a thousand degrees per iteration and ends 0.80 m from the target, while DLS smoothly stretches the arm towards it and ends 0.30 m away, exactly the real shortfall ($1.20-0.90$). A real system should first check whether the target is in the workspace and raise a warning instead of trying on.