Kinematics

Forward Kinematics

Forward kinematics (FK) maps joint angles θ = (θ₁ … θ₆) to the end-effector pose T ∈ SE(3).

Each joint’s transformation is a 4×4 homogeneous matrix constructed from the DH parameters:


T_i = Rot_x(α_{i-1}) · Trans_x(a_{i-1}) · Rot_z(θ_i) · Trans_z(d_i)

The full FK is the chain product:


T_0_6 = T_0_1 · T_1_2 · T_2_3 · T_3_4 · T_4_5 · T_5_6

Python implementation


import numpy as np
 
def dh_matrix(alpha, a, d, theta):
    """Single DH transformation matrix (modified convention)."""
    ca, sa = np.cos(alpha), np.sin(alpha)
    ct, st = np.cos(theta), np.sin(theta)
    return np.array([
        [ ct,    -st,    0,    a   ],
        [ st*ca,  ct*ca, -sa, -sa*d],
        [ st*sa,  ct*sa,  ca,  ca*d],
        [ 0,      0,      0,   1   ],
    ])
 
# DH table: (alpha, a, d)  — theta added at runtime
DH = [
    (0,          0,    0.120),   # joint 1
    (np.pi/2,    0,    0.000),   # joint 2
    (-np.pi/2,   0,    0.000),   # joint 3
    (np.pi/2,    0.250, 0.000),  # joint 4
    (-np.pi/2,   0.230, 0.000),  # joint 5
    (np.pi/2,    0,    0.060),   # joint 6
]
 
def forward_kinematics(q):
    """
    q: array of 6 joint angles (radians)
    Returns: 4×4 end-effector pose in base frame
    """
    T = np.eye(4)
    for i, (alpha, a, d) in enumerate(DH):
        T = T @ dh_matrix(alpha, a, d, q[i])
    return T

Inverse Kinematics

Inverse kinematics (IK) finds joint angles θ that achieve a target pose T_des.

For this 6-DOF configuration, an analytical + numerical hybrid is used:

Joints 1–3 (shoulder): geometric closed-form solution for wrist centre position
Joints 4–6 (wrist): Euler angle decomposition from the remaining rotation

For a quick numerical-only IK (good for prototyping), use the Jacobian pseudo-inverse:


def jacobian(q, eps=1e-6):
    """Numerical Jacobian (6×6)."""
    T0 = forward_kinematics(q)
    J = np.zeros((6, 6))
    for i in range(6):
        dq = np.zeros(6)
        dq[i] = eps
        T1 = forward_kinematics(q + dq)
        # linear velocity columns
        J[:3, i] = (T1[:3, 3] - T0[:3, 3]) / eps
        # angular velocity columns (axis-angle approximation)
        R_delta = T1[:3, :3] @ T0[:3, :3].T
        J[3, i] = R_delta[2, 1] / eps
        J[4, i] = R_delta[0, 2] / eps
        J[5, i] = R_delta[1, 0] / eps
    return J
 
def inverse_kinematics(T_des, q0, max_iter=200, tol=1e-4):
    """
    Iterative IK via damped least-squares Jacobian.
    T_des: 4×4 target pose
    q0:    initial joint angles
    """
    q = q0.copy()
    lam = 0.01  # damping factor
    for _ in range(max_iter):
        T = forward_kinematics(q)
        # position error
        dp = T_des[:3, 3] - T[:3, 3]
        # orientation error (axis-angle)
        R_err = T_des[:3, :3] @ T[:3, :3].T
        dR = np.array([R_err[2, 1], R_err[0, 2], R_err[1, 0]])
        err = np.concatenate([dp, dR])
        if np.linalg.norm(err) < tol:
            break
        J = jacobian(q)
        # damped least-squares
        dq = J.T @ np.linalg.solve(J @ J.T + lam**2 * np.eye(6), err)
        q += dq
    return q

Damped least-squares (λ = 0.01) avoids singularity blow-up near wrist-flip configurations. Reduce λ for higher accuracy away from singularities.

Workspace

The reachable workspace is approximately a torus centred at the shoulder with:

Inner radius ≈ 50 mm (arm folded)
Outer radius ≈ 540 mm (arm fully extended)
Height range ≈ ±480 mm from shoulder centre

Dexterous workspace (all 6 DOF controllable) is roughly a sphere of radius 300 mm centred 300 mm in front of the shoulder.