Learn › Engineering

Robot arm: forward and inverse kinematics

A robot arm is a chain of links joined by motors, from a fixed base to a tool (the end effector). Programming it requires going back and forth between the motor angles and the position of the tool: that is forward and inverse kinematics. The 3D robot arm tool does it with the Denavit–Hartenberg convention, the one used in robotics courses.

Go to the simulator

Joints and degrees of freedom

Each joint of a serial arm is a revolute (R, it turns) or a prismatic one (P, it slides), and contributes one degree of freedom. To put the tool at any point in space you need 3; to also orient it as you wish, 6. That is why industrial arms have 6 joints: three to bring the wrist into place and three (the spherical wrist) to orient the tool.

Configuration	Joints	Typical use
Cartesian (gantry)	PPP	3D printers, CNC machines, palletizing
SCARA	RRPR	fast assembly on a table
Anthropomorphic	RRR	base, shoulder and elbow, like a human arm
6R with a spherical wrist	RRRRRR	welding, painting, general handling

Denavit–Hartenberg parameters

To chain the joints together, a frame is attached to each one, with the axis $z_{i}$ along the axis of rotation (or sliding). The Denavit–Hartenberg convention describes the step from one frame to the next with just four numbers:

Parameter	What it measures	Variable if…
$θ_{i}$	rotation about $z_{i - 1}$	the joint is R
$d_{i}$	offset along $z_{i - 1}$	the joint is P
$a_{i}$	length of the common normal (along the new $x_{i}$ )	—
$α_{i}$	twist between $z_{i - 1}$ and $z_{i}$ about $x_{i}$	—

A_{i} = Rot_{z} (θ_{i}) Tras_{z} (d_{i}) Tras_{x} (a_{i}) Rot_{x} (α_{i}) = cos θ_{i} sin θ_{i} 00 - sin θ_{i} cos α_{i} cos θ_{i} cos α_{i} sin α_{i} 0 sin θ_{i} sin α_{i} - cos θ_{i} sin α_{i} cos α_{i} 0 a_{i} cos θ_{i} a_{i} sin θ_{i} d_{i} 1

It is a 4 × 4 homogeneous matrix: its 3 × 3 block is a rotation and its last column, a translation. Multiplying them in order gives the pose of the tool relative to the base:

T_{0}^{n} = A_{1} A_{2} \dots A_{n} = [x_{n} 0 y_{n} 0 z_{n} 0 p 1]

A typical mistake is to apply the matrix with another order of rotations: there is a «modified» DH variant with the parameters shifted by one index. The simulator uses the classic one,

Rot_{z} Tras_{z} Tras_{x} Rot_{x}

, and says so below the table.

Forward kinematics

From the motor angles you get the position of the tool: it is just multiplying matrices. In the planar two-link arm, the product reduces to adding the two vectors:

x = a_{1} cos q_{1} + a_{2} cos (q_{1} + q_{2}) y = a_{1} sin q_{1} + a_{2} sin (q_{1} + q_{2})

Planar 2R arm of example 1.

q_{2}

is measured from the extension of the first link.

Inverse kinematics

The opposite problem, which angles put the tool at a given point, is harder: it is nonlinear and may have several solutions or none. In the 2R it is solved with the law of cosines:

cos q_{2} = \frac{x ^{2} + y ^{2} - a _{1}^{2} - a _{2}^{2}}{2 a _{1} a _{2}} q_{1} = atan2 (y, x) - atan2 (a_{2} sin q_{2}, a_{1} + a_{2} cos q_{2})

If $∣ cos q_{2} ∣ > 1$ , the point is outside the workspace (the ring between $∣ a_{1} - a_{2} ∣$ and $a_{1} + a_{2}$ ).
If it is inside, there are two solutions, $\pm q_{2}$ : elbow up and elbow down.
For general arms there is not always a formula: it is solved numerically, correcting the angles with the Jacobian until the position error vanishes. The simulator uses Levenberg–Marquardt and starts from the current pose, so it finds the solution closest to it.

Workspace of the 2R and the two solutions to reach (0.6, 0.3) m (example 3).

Jacobian and singularities

The Jacobian $J$ relates the motor velocities to that of the tool: $v = J_{v} \dot{q}$ , $ω = J_{ω} \dot{q}$ . Each column comes from the geometry: for an R joint, $J_{v, i} = z_{i - 1} \times (p - o_{i - 1})$ and $J_{ω, i} = z_{i - 1}$ ; for a P joint, $J_{v, i} = z_{i - 1}$ .

When its columns become dependent, the arm is at a singularity: there are directions in which the tool cannot move however fast the motors turn, and near them the required joint velocities blow up. In the 2R:

det J = a_{1} a_{2} sin q_{2} = 0 ⟺ q_{2} = 0° (estirado) o 180° (plegado)

The manipulability $w = det (J_{v} J_{v}^{T})$ measures how far you are from a singularity (with fewer than three joints $det (J_{v}^{T} J_{v})$ is used, which in the 2R is $∣ det J ∣$ ). The Jacobian also gives the static torques: to hold a force $F$ at the tool you need $τ = J_{v}^{T} F$ .

Trajectories

A robot does not jump from one pose to another: each joint follows a motion law that starts and stops smoothly. Moving each joint from its start to its end value with the fifth-degree polynomial $s (τ) = 10 τ^{3} - 15 τ^{4} + 6 τ^{5}$ ( $τ = t / T$ ), the velocity and acceleration start and end at zero. If the tool must follow a line or a circle in space, inverse kinematics is solved at every instant of the path.

Fifth-degree profile: position, velocity and acceleration (in units of the total travel and of

T

Common mistakes

Mixing classic and modified DH, or multiplying the matrices in the reverse order.
Measuring $q_{2}$ from the $x$ axis instead of from the extension of the first link.
Using $arctan (y / x)$ instead of $atan2 (y, x)$ : the quadrant is lost.
Keeping a single inverse-kinematics solution without looking at the joint limits (the other one may be the only feasible one).
Multiplying the Jacobian by velocities in °/s: the columns of R joints need rad/s.

Worked examples

Example 1Forward kinematics and Jacobian of a 2R arm

A planar arm has

a_{1} = 0.5

m and

a_{2} = 0.4

m. With

q_{1} = 30°

and

q_{2} = 45°

, find the position of the tool, the position Jacobian and its determinant.

Solution

$q_{1} + q_{2} = 75°$ . Position: $x = 0.5 cos 30° + 0.4 cos 75° = 0.4330 + 0.1035 = 0.5365$ m; $y = 0.5 sin 30° + 0.4 sin 75° = 0.25 + 0.3864 = 0.6364$ m.
Differentiating $x$ and $y$ with respect to $q_{1}$ and $q_{2}$ : $J = [- a_{1} s_{1} - a_{2} s_{12} a_{1} c_{1} + a_{2} c_{12} - a_{2} s_{12} a_{2} c_{12}] = [- 0.6364 0.5365 - 0.3864 0.1035]$ .
Notice that the first column is $(- y, x)$ : turning the base moves the tool perpendicular to the line joining it to the base.
$det J = a_{1} a_{2} sin q_{2} = 0.5 \cdot 0.4 \cdot sin 45° = 0.1414$ m²: far from a singularity.

p = (0.5365, 0.6364)

m and

det J = 0.1414

m².

Solve it in Simuveliq

The button opens the 2R arm stopped at $q_{1} = 30°$ , $q_{2} = 45°$ (in Engineering → 3D robot arm, the «Planar 2R» example is the same arm, but moving).
The End-effector position card gives 0.537, 0.636, 0 m and Manipulability 0.141 m². In «Forward kinematics step by step», the matrix $T_{0}^{2}$ has the position (0.5365, 0.6364, 0) in its last column and the «Jacobian and velocities» block the two columns above.
Change $q_{2}$ to 0° in Home position and limits: the manipulability drops to 0 and «near a singularity» appears.

Open in the simulator →

Example 2Denavit–Hartenberg of an anthropomorphic arm

An RRR arm has this DH table: joint 1 with

d_{1} = 0.4

m and

α_{1} = 90°

; joint 2 with

a_{2} = 0.5

m; joint 3 with

a_{3} = 0.4

m (the other parameters, zero). With

q = (30°, 40°, - 70°)

, find the position of the tool.

Solution

$A_{1}$ raises the shoulder height ( $d_{1} = 0.4$ ) and, with $α_{1} = 90°$ , makes axis $z_{1}$ horizontal: joints 2 and 3 turn in a vertical plane that the base orients with $q_{1}$ .
In that plane it works like a 2R: horizontal reach $r = 0.5 cos 40° + 0.4 cos (40° - 70°) = 0.3830 + 0.3464 = 0.7294$ m and height $z = 0.4 + 0.5 sin 40° + 0.4 sin (- 30°) = 0.4 + 0.3214 - 0.2 = 0.5214$ m.
The base turns the plane by $q_{1} = 30°$ : $x = r cos 30° = 0.6317$ m and $y = r sin 30° = 0.3647$ m. It is the last column of $T_{0}^{3} = A_{1} A_{2} A_{3}$ .

p = (0.6317, 0.3647, 0.5214)

Solve it in Simuveliq

The button opens the 3R anthropomorphic arm stopped at that pose. The Denavit–Hartenberg parameters table has $d_{1}$ , $α_{1}$ , $a_{2}$ and $a_{3}$ .
«Forward kinematics step by step» shows the table with the angles of the instant, each $A_{i}$ and $T_{0}^{3}$ with the position (0.6317, 0.3647, 0.5214) in its last column; the End-effector position card gives the same.

Open in the simulator →

Example 3Inverse kinematics of the 2R arm

With the arm of example 1, which angles put the tool at

(0.6, 0.3)

Solution

Distance to the point: $r^{2} = 0.36 + 0.09 = 0.45$ m², inside the ring from 0.1 to 0.9 m.
$cos q_{2} = \frac{0.45 - 0.25 - 0.16}{2 \cdot 0.5 \cdot 0.4} = 0.1 \Rightarrow q_{2} = \pm 84.26°$ .
With $q_{2} = + 84.26°$ : $q_{1} = atan2 (0.3, 0.6) - atan2 (0.398, 0.54) = 26.57° - 36.39° = - 9.83°$ (elbow down).
With $q_{2} = - 84.26°$ : $q_{1} = 26.57° + 36.39° = 62.96°$ (elbow up). Both are valid; the robot chooses depending on where it starts from and on its limits.

(q_{1}, q_{2}) = (- 9.83°, 84.26°)

(62.96°, - 84.26°)

Solve it in Simuveliq

The button opens the arm with the Point to point (joint space) motion toward (0.6, 0.3, 0), stopped at the end of the move.
It starts from (30°, 45°), so the numerical method reaches the nearest solution: the «Forward kinematics step by step» table shows $q_{1} = - 9.826°$ and $q_{2} = 84.26°$ , and the tool at (0.6, 0.3, 0).
Change the home position to $q_{2} = - 45°$ and you will see it get there the other way, elbow up.

Open in the simulator →

Robot arm: forward and inverse kinematics

Joints and degrees of freedom

Denavit–Hartenberg parameters

Forward kinematics

Inverse kinematics

Jacobian and singularities

Trajectories

Common mistakes

Worked examples

Example 1Forward kinematics and Jacobian of a 2R arm

Solution

Solve it in Simuveliq

Example 2Denavit–Hartenberg of an anthropomorphic arm

Solution

Solve it in Simuveliq

Example 3Inverse kinematics of the 2R arm

Solution

Solve it in Simuveliq

Related topics