AR4 Robot Arm Kinematics

The chapter so far built the machinery in the abstract: the configuration space a robot lives in, the homogeneous coordinates that let a single

4\times4

matrix carry both rotation and translation at once, and the motion representations that describe orientation. This section spends that machinery on a real six-axis arm, the AR4, and works through its forward kinematics (where the hand is, given the joint angles) and inverse kinematics (which joint angles place the hand at a desired pose).

Forward and inverse kinematics of the AR4 arm, by Chris Annin (Annin Robotics), whose open-source Denavit-Hartenberg models this section builds on. The AR4 is an open-source arm with six revolute joints. Its configuration is a single vector

\mathbf{q} = (q_1,\dots,q_6)

of joint angles, a point in a six-dimensional configuration space bounded by the mechanical joint limits. Everything below is a function of that vector.

import numpy as np

pi = np.pi

Denavit-Hartenberg parameters

A serial arm is a chain of links connected by joints. The Denavit-Hartenberg (DH) convention is the standard bookkeeping that attaches a coordinate frame to every link and summarises the geometry between consecutive frames with just four numbers per joint: the joint angle

\theta

, the link offset

d

, the link length

a

, and the link twist

\alpha

. Each row of the table is nothing more than one homogeneous transform, so the whole arm is the product of six of the

4\times4

matrices from the homogeneous coordinates section. There are two DH conventions in common use, and the AR4 ships with both. Standard (classic) DH places each frame at the far end of its link; modified (Craig) DH places it at the near end. They describe the same physical arm and produce the same hand pose, but the numbers in the table differ, which is a frequent source of confusion when reading someone else’s model. The table below is the AR4 in the standard convention. The

\theta

column is the fixed offset added to each joint variable, so the joint you command is

q_i

and the angle the transform sees is

q_i + \theta_i

# AR4 standard-DH table: columns are (theta_offset, alpha, d, a), angles in radians, lengths in mm.
STD_DH = np.array([
    [0.0,     -pi/2, 169.77,  64.2],   # joint 1
    [-pi/2,    0.0,    0.0,   305.0],   # joint 2
    [ pi,      pi/2,   0.0,     0.0],   # joint 3
    [0.0,     -pi/2, 222.63,   0.0],   # joint 4
    [ pi/2,    pi/2,   0.0,     0.0],   # joint 5
    [0.0,      0.0,   41.0,     0.0],   # joint 6
])

# Mechanical joint limits (degrees): (min, max) per joint.
JOINT_LIMITS_DEG = np.array([
    [-170, 170], [-42, 90], [-89, 52], [-165, 165], [-105, 105], [-155, 155],
])

The DH transform

For the standard convention, the transform from frame

i-1

to frame

i

is a rotation about

z

\theta

, a translation along

z

d

, a translation along

x

a

, and a rotation about

x

\alpha

, composed into the single matrix

A_i= \begin{bmatrix} \cos\theta & -\sin\theta\cos\alpha & \sin\theta\sin\alpha & a\cos\theta\\ \sin\theta & \cos\theta\cos\alpha & -\cos\theta\sin\alpha & a\sin\theta\\ 0 & \sin\alpha & \cos\alpha & d\\ 0 & 0 & 0 & 1 \end{bmatrix}.

The top-left

3\times3

block is the orientation of link

i

relative to link

i-1

; the last column is its position. This is exactly the homogeneous transform of the earlier section, specialised to the four DH numbers.

def dh_standard(theta, d, a, alpha):
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), np.sin(alpha)
    return np.array([
        [ct, -st * ca,  st * sa, a * ct],
        [st,  ct * ca, -ct * sa, a * st],
        [0.0,      sa,       ca,      d],
        [0.0,     0.0,      0.0,    1.0],
    ])

def forward_kinematics(q, dh=STD_DH):
    '''Pose of the end-effector frame given joint vector q (radians).'''
    T = np.eye(4)
    for i in range(6):
        theta = q[i] + dh[i, 0]
        T = T @ dh_standard(theta, dh[i, 2], dh[i, 3], dh[i, 1])
    return T

Forward kinematics

Forward kinematics is the product

T^0_6=A_1 A_2 \cdots A_6

. It maps a point

\mathbf{q}

in the six-dimensional configuration space to a single hand pose: a position and an orientation in the world frame. The map is smooth and, for a given arm, exact, so it is the natural ground truth to check an implementation against. At the home configuration

\mathbf{q}=\mathbf 0

the AR4 hand sits at a known point, which is what the next cell confirms.

q_home = np.zeros(6)
T_home = forward_kinematics(q_home)

print("end-effector position (mm):", np.round(T_home[:3, 3], 2))
print("expected from the AR4 model:", [286.83, 0.0, 433.77])

end-effector position (mm): [286.83  -0.   433.77]
expected from the AR4 model: [286.83, 0.0, 433.77]

End-effector orientation

The hand pose carries an orientation as well as a position. The rotation block of

T^0_6

can be read out in any of the motion representations; a

Z\!-\!Y\!-\!X

Euler triple (yaw, pitch, roll) is convenient because it matches how the arm’s controller reports the tool pose.

def euler_zyx_deg(R):
    '''Yaw (Rz), pitch (Ry), roll (Rx) in degrees from a rotation matrix.'''
    pitch = np.arctan2(-R[2, 0], np.hypot(R[0, 0], R[1, 0]))
    yaw   = np.arctan2(R[1, 0], R[0, 0])
    roll  = np.arctan2(R[2, 1], R[2, 2])
    return np.rad2deg([yaw, pitch, roll])

print("home orientation (Rz, Ry, Rx) deg:", np.round(euler_zyx_deg(T_home[:3, :3]), 2))

home orientation (Rz, Ry, Rx) deg: [180.   0. 180.]

Seeing the arm

Reading a pose off a matrix is abstract; drawing the arm makes the chain concrete. Taking the running product

A_1, A_1 A_2, \dots

gives the position of every joint, and connecting them in order traces the physical links from the base to the hand. The figure shows the AR4 at its home pose and at a bent configuration.

def joint_positions(q, dh=STD_DH):
    '''World positions of the base and each successive joint frame origin.'''
    T = np.eye(4)
    pts = [T[:3, 3].copy()]
    for i in range(6):
        theta = q[i] + dh[i, 0]
        T = T @ dh_standard(theta, dh[i, 2], dh[i, 3], dh[i, 1])
        pts.append(T[:3, 3].copy())
    return np.array(pts)

Standard versus modified DH

The same arm, written in the modified (Craig) convention, uses a different table because each frame sits at the near end of its link instead of the far end, and the twist and length of the previous link move into each row. The transform is reshaped accordingly. Neither table is more correct; they are two encodings of one geometry, and each reproduces the hand pose its own spreadsheet reports. Recognising which convention a model uses is the practical skill, because mixing the two silently produces a wrong arm.

# AR4 modified-DH table: columns (theta_offset, alpha_{i-1}, d, a_{i-1}).
MOD_DH = np.array([
    [0.0,            0.0,   169.77,  0.0],
    [-pi/2,         -pi/2,    0.0,  64.2],
    [np.deg2rad(-89), 0.0,    0.0, 305.0],
    [0.0,           -pi/2, 222.63,  0.0],
    [ pi/2,          pi/2,    0.0,   0.0],
    [ pi,           -pi/2,  36.25,   0.0],
])

def dh_modified(theta, d, a, alpha):
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), np.sin(alpha)
    return np.array([
        [   ct,    -st,  0.0,      a],
        [st * ca, ct * ca, -sa, -sa * d],
        [st * sa, ct * sa,  ca,  ca * d],
        [0.0, 0.0, 0.0, 1.0],
    ])

def fk_modified(q, dh=MOD_DH):
    T = np.eye(4)
    for i in range(6):
        theta = q[i] + dh[i, 0]
        T = T @ dh_modified(theta, dh[i, 2], dh[i, 3], dh[i, 1])
    return T

T_mod = fk_modified(np.zeros(6))
print("modified-DH home position (mm):", np.round(T_mod[:3, 3], 2))
print("expected from the AR4 modified model:", [104.33, 0.0, 696.73])

modified-DH home position (mm): [104.33  -0.   696.73]
expected from the AR4 modified model: [104.33, 0.0, 696.73]

Inverse kinematics

Forward kinematics is a direct product; inverse kinematics is the harder inverse question, and for a general pose it has no unique answer. The AR4 has a spherical wrist, the last three joint axes intersecting at a point, which classically lets the problem decouple: the wrist-centre position fixes the first three joints, and the remaining orientation fixes the last three. Here the same solution is reached by iteration, which needs no special-case algebra and generalises to arms without that structure. Starting from a guess, each step measures the pose error, both the position gap and the orientation gap as a rotation vector, and corrects the joints along the Jacobian of the forward map. The loop converges to joints whose forward kinematics reproduce the target, which is the check applied below.

def rotation_vector(R):
    '''Axis-times-angle vector of a rotation matrix.'''
    angle = np.arccos(np.clip((np.trace(R) - 1) / 2, -1, 1))
    if angle < 1e-9:
        return np.zeros(3)
    axis = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]])
    return angle / (2 * np.sin(angle)) * axis

def jacobian(q, eps=1e-6):
    J = np.zeros((6, 6))
    T = forward_kinematics(q)
    for i in range(6):
        dq = q.copy(); dq[i] += eps
        Td = forward_kinematics(dq)
        J[:3, i] = (Td[:3, 3] - T[:3, 3]) / eps
        J[3:, i] = rotation_vector(Td[:3, :3] @ T[:3, :3].T) / eps
    return J

def inverse_kinematics(T_target, q_init, iters=200, damping=1e-6, tol=1e-6):
    q = q_init.copy()
    for _ in range(iters):
        T = forward_kinematics(q)
        err = np.concatenate([
            T_target[:3, 3] - T[:3, 3],
            rotation_vector(T_target[:3, :3] @ T[:3, :3].T),
        ])
        if np.linalg.norm(err) < tol:
            break
        J = jacobian(q)
        q = q + np.linalg.solve(J.T @ J + damping * np.eye(6), J.T @ err)
    return q

# Round-trip test: pick joints, compute the pose, recover the joints from the pose alone.
q_true = np.deg2rad([30, -20, 15, 40, -50, 60])
T_goal = forward_kinematics(q_true)

q_solved = inverse_kinematics(T_goal, np.zeros(6))
pos_error = np.linalg.norm(forward_kinematics(q_solved)[:3, 3] - T_goal[:3, 3])

print("recovered joints (deg):", np.round(np.rad2deg(q_solved), 2))
print("position error (mm):   ", round(pos_error, 6))

recovered joints (deg): [ 30. -20.  15.  40. -50.  60.]
position error (mm):    0.0

From joint angles to motor steps

Kinematics stops at joint angles, but the arm is driven by stepper motors, and the firmware speaks in steps. Each joint has its own reduction, the product of gearbox and pulley ratios, and a fixed number of microsteps per motor revolution, so a commanded angle becomes an integer count of steps. The same joint vector that produced a hand pose becomes the integer step counts the controller sends to the drivers.

# AR4-MK steps per degree at each joint (from the motor-step model, default build).
STEPS_PER_DEG = np.array([88.8889, 111.1111, 111.1111, 99.5556, 43.7205, 44.4444])

def joint_steps(q_deg):
    '''Integer stepper counts for a joint vector given in degrees.'''
    return np.round(q_deg * STEPS_PER_DEG).astype(int)

q_deg = np.array([30, -20, 15, 40, -50, 60])
print("joint angles (deg):", q_deg)
print("motor steps:       ", joint_steps(q_deg))

joint angles (deg): [ 30 -20  15  40 -50  60]
motor steps:        [ 2667 -2222  1667  3982 -2186  2667]

From the model to the real robot

Everything here was derived from the arm’s Denavit-Hartenberg table: a compact, convention-defined summary of the geometry. The AR4 kinematics lab takes the same arm from the other direction, parsing its as-built URDF, and reconciles the two descriptions to see where they agree and where a real millimeter-scale discrepancy hides.

Edit this page on GitHub or file an issue.

Robotics Systems

Kinematics & Dynamics

Sensor Models

Systems Integration

AR4 Robot Arm Kinematics

Denavit-Hartenberg parameters

The DH transform

Forward kinematics

End-effector orientation

Seeing the arm

Standard versus modified DH

Inverse kinematics

From joint angles to motor steps

From the model to the real robot

​Denavit-Hartenberg parameters

​The DH transform

​Forward kinematics

​End-effector orientation

​Seeing the arm

​Standard versus modified DH

​Inverse kinematics

​From joint angles to motor steps

​From the model to the real robot

Denavit-Hartenberg parameters

The DH transform

Forward kinematics

End-effector orientation

Seeing the arm

Standard versus modified DH

Inverse kinematics

From joint angles to motor steps

From the model to the real robot