Forward and Inverse

Kinematics

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Introduction
• The problem of kinematics is to describe the motion of the manipulator
without consideration of the forces and torques causing the motion.
• The kinematic description is therefore a geometric one.
• The problem of forward kinematics, which is to determine the position
and orientation of the end-effector given the values for the joint variables
of the robot.
• The inverse kinematics problem is to determine the values of the joint
variables given the end-effector position and orientation.

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Kinematic chains
A robot manipulator is composed of a set of links connected together
by joints.
• A revolute joint is like a hinge and allows a relative rotation about a
single axis.
• A prismatic joint permits a linear motion along a single axis, namely an
extension or retraction.
From now on it is assumed throughout that all joints have only a single
degree-of-freedom

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Kinematic chains

• A robot manipulator with 𝑛 joints will have 𝑛 + 1 links, since each joint
connects two links.
• We number the joints from 1 to 𝑛, and we number the links from 0 to 𝑛,
starting from the base.
• By this convention, joint 𝑖 connects link 𝑖 − 1 to link 𝑖. Joint 𝑖 is fixed to
link 𝑖 − 1. When joint 𝑖 is actuated, link 𝑖 moves.
• With the 𝑖𝑡ℎ joint, we associate a joint variable, denoted by 𝑞𝑖 :

𝜃𝑖 𝑖𝑓 𝑗𝑜𝑖𝑛𝑡 𝑖 𝑖𝑠 𝑟𝑒𝑣𝑜𝑙𝑢𝑡𝑒
𝑞𝑖 = {
𝑑𝑖 𝑖𝑓 𝑗𝑜𝑖𝑛𝑡 𝑖 𝑖𝑠 𝑝𝑟𝑖𝑠𝑚𝑎𝑡𝑖𝑐

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Kinematic chains

We attach a coordinate frame rigidly to each link. In particular, we attach

𝑜𝑖 𝑥𝑖 𝑦𝑖 𝑧𝑖 to link 𝑖.

Whatever motion the robot executes, the coordinates of each point on link
𝑖 are constant when expressed in the 𝑖 𝑡ℎ coordinate frame.

Suppose 𝐴𝑖 is the homogeneous transformation matrix that expresses

the position and orientation of 𝑜𝑖 𝑥𝑖 𝑦𝑖 𝑧𝑖 with respect 𝑜𝑖−1 𝑥𝑖−1 𝑦𝑖−1 𝑧𝑖−1 , if all
joints are either revolute or prismatic it means that 𝐴𝑖 is a function of only
a single joint variable, namely 𝑞𝑖

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Kinematic chains

the homogeneous transformation matrix that expresses the position and

orientation of 𝑜𝑗 𝑥𝑗 𝑦𝑗 𝑧𝑗 with respect to 𝑜𝑖 𝑥𝑖 𝑦𝑖 𝑧𝑖 is called, a transformation
matrix 𝑻𝒊𝒋 y is denoted by:

the position and orientation of the end-effector in the inertial frame are
given by

Each homogeneous transformation Ai is of the form

Hence

Robótica A
Dr. Mauro Eduardo Maya Méndez
 The Denavit-Hartenberg Convention

In the DH convention, each homogeneous transformation Ai is represented

as a product of four basic transformations.

• 𝑎𝑖 = 𝑙𝑖𝑛𝑘 𝑙𝑒𝑛𝑔𝑡ℎ.
• 𝛼𝑖 = 𝑙𝑖𝑛𝑘 𝑡𝑤𝑖𝑠𝑡
• 𝑑𝑖 = 𝑙𝑖𝑛𝑘 𝑜𝑓𝑓𝑠𝑒𝑡
• 𝜃𝑖 = joint angle

Robótica A
Dr. Mauro Eduardo Maya Méndez
 DH Coordinate Frame Assumptions

• DH1: the axis 𝑥1 is perpendicular

to the axis 𝑧0 .
• DH2: the axis 𝑥1 intersects the
axis 𝑧0 .

Under these conditions, we claim

that there exist unique values for
the parameters, such that

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Assigning the coordinate frames
In order to set up frame 𝑖 it is necessary to consider three cases:
• 𝒛𝒊−𝟏 and 𝒛𝒊 are not coplanar. There exists a unique line segment
perpendicular to both 𝑧𝑖−1 and 𝑧𝑖 such that defines 𝑥1 .
• 𝒛𝒊−𝟏 is parallel to 𝒛𝒊 . There are infinitely many common normals, we are
free to choose the origin 𝑜𝑖 anywhere along 𝑧𝑖 .
• 𝒛𝒊−𝟏 intersects 𝒛𝒊 . The 𝑥𝑖 − 𝑎𝑥𝑖𝑠 is chosen normal to the plane formed
by 𝑧𝑖 y 𝑧𝑖−1 .

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Planar Elbow Manipulator

• The axes 𝑧0 y 𝑧1 are normal to the

page.
• Establish the base frame 𝑜0 𝑥0 𝑦0 𝑧0
as shown.
• The direction of the 𝑥0 axis is
completely arbitrary.
• Establish the 𝑜1 𝑥1 𝑦1 𝑧1 frame is
fixed by the convention DH.
• The final frame 𝑜2 𝑥2 𝑦2 𝑧2 is fixed
by choosing the origin 𝑥2 al final
del link 2.

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Planar Elbow Manipulator

Parameters DH

The T-Matrices are thus given by

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Three-link Cylindrical Robot

• Establish 𝑜0 as shown at the joint

1.
• The origin 𝑜0 en 𝑧0 as well the
direction of 𝑥0 -axis are arbitrary.
• Our choice of 𝑜0 is the most
natural, but 𝑜0 could just as well
be placed at joint 2.
• The axis 𝑥0 is chosen normal to
the page.
• The 𝑥1 axis is normal to the page
when 𝜃1 = 0 but, of course its
direction will change since 𝜃1 is
variable.

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Three-link Cylindrical Robot
Parameters DH

The T-Matrices are thus given by

Robótica A
Dr. Mauro Eduardo Maya Méndez
Spherical Wrist

Robótica A
Dr. Mauro Eduardo Maya Méndez
Cylindrical Manipulator with Spherical Wrist

Robótica A
Dr. Mauro Eduardo Maya Méndez
Stanford Manipulator

Robótica A
Dr. Mauro Eduardo Maya Méndez
Stanford Manipulator

Robótica A
Dr. Mauro Eduardo Maya Méndez
SCARA Manipulator

Robótica A
Dr. Mauro Eduardo Maya Méndez
SCARA Manipulator

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Inverse kinematics

The general problem of inverse The equation results in twelve

kinematics can be stated as nonlinear equations in n unknown
follows. Given a 4𝑥4 variables, which can be written as
homogeneous transformation

With 𝑅 = 𝑆𝑂(3), find (one or all)

solutions of the equation Where 𝑇𝑖𝑗 and ℎ𝑖𝑗 refer to the
twelve nontrivial entries of 𝑇𝑛0 and
H.

where

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Inverse kinematics

Example
We must solve the following
Recall the Stanford manipulator. simultaneous set of nonlinear
Suppose that the desired position trigonometric equations:
and orientation of the final frame
are given by

To find the corresponding joint

variables 𝜃1 , 𝜃2 , 𝑑3 , 𝜃4 , 𝜃5 𝑎𝑛𝑑 𝜃6 .

One solution is given by:

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Kinematic Decoupling
The origin of the tool frame given It’s necessary and sufficient that the
by 𝑜) is simply obtained by a wrist center 𝑜𝑐 have coordinates
translation of distance 𝑑6 along 𝑧5
from 𝑜𝑐 . The axes 𝑧5 and 𝑧6 are
the same axis, and the third
column of 𝑅 expresses the and the orientation of the frame
direction of 𝑧6 . Therefore, we have 𝑜6 𝑥6 𝑦6 𝑧6 with respect to the base be
given by 𝑅. The components of the
end-effector position 𝑜 are 𝑜𝑥 , 𝑜𝑦 , 𝑜𝑧
and the components of the wrist
center 𝑜𝑐0 are 𝑥𝑐 , 𝑦𝑐 , 𝑧𝑐 , gives the
relationship
in order to have the end-effector of 𝑥𝑐 𝑜𝑥 − 𝑑6 𝑟13
the robot at the point with 𝑦𝑐 = 𝑜𝑦 − 𝑑6 𝑟23
coordinates given by o and with
𝑧𝑐 𝑜𝑧 − 𝑑6 𝑟33
the orientation of the end-effector
given by 𝑅 = 𝑟𝑖𝑗 . Now, we may find the values of the
first three joint variables.
Robótica A
Dr. Mauro Eduardo Maya Méndez
 Kinematic Decoupling
We can now determine the
orientation of the end-effector The idea of kinematic decoupling is
relative to the frame 𝑜3 𝑥3 𝑦3 𝑧3 from
the expression

As we shall see, the final three

joint angles can then be found as
a set of Euler angles
corresponding to 𝑅63 . Note that the
right hand side is completely
known since 𝑅 is given and 𝑅30
can be calculated once the first
three joint variables are known.

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Inverse Position: A Geometric Approch

In general the complexity of the

We restrict our treatment to the
inverse kinematics problem increases
geometric approach for two
with the number of nonzero link
reasons.
parameters.
First, most present manipulator
For most manipulators, many of the 𝑎𝑖
designs are kinematically simple, 𝜋
it is partly due to the difficulty of , 𝑑𝑖 are zero, the 𝛼𝑖 are 0 or ± , etc.
2
the general inverse kinematics In these cases especially, a geometric
problem. Second, there are few is the simplest and most natural.
techniques that can handle the The general idea of the geometric
general in verse kinematics approach is to solve for joint variable
problem for arbitrary 𝑞𝑖 by projecting the manipulator onto
configurations. the 𝑥𝑖−1 − 𝑦𝑖−1 plane and solving a
simple trigonometry problem.

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Articulated Configuration.
Consider the elbow manipulator Projection of the wrist center onto
shown next, with the components 𝑥0 − 𝑦0 plane
of 𝑜𝑐0 denoted by 𝑥𝑐 , 𝑦𝑐 , 𝑧𝑐 . We
project 𝑜𝑐 onto the 𝑥0 − 𝑦0 plane

We see from this projection that

Note that a sencond valid solution for

𝜃1 is

this will, in turn, lead to different

solutions for 𝜃2 and 𝜃3 .

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Articulated Configuration.
The solutions for 𝜃1 , are valid If there is 𝑑 ≠ 0 as shown next, then
unless 𝑥𝑐 = 𝑦𝑐 = 0. In this case is the wrist center cannot intersect 𝑧0 .
un undefined and the manipulator
is in a singular configuration,
shown

In this case, depending on how the

DH parameters have been assigned,
We will have 𝑑2 = 𝑑 or 𝑑3 = 𝑑. In this
case, there will, in general, be only
two solutions for 𝜃1 .
There are infinitely many solutions
for 𝜃1 when 𝑜𝑐 intersects 𝑧0 .
Robótica A
Dr. Mauro Eduardo Maya Méndez
 Articulated Configuration.
These correspond to the so-called Where
left arm configuration as shown
next

we see geometrically that

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Articulated Configuration.
Right arm configuration is shown The second solution, given by the
next right arm configuration is

To see this, note that

Which together imply that

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Articulated Configuration.
To find the angles 𝜃2 , 𝜃3 for the We can apply the law of cosines to
elbow manipulator, given 𝜃1 , we obtain:
consider the plane formed by the
second and third links as shown
next

Since 𝑟 2 = 𝑥𝑐2 + 𝑦𝑐2 − 𝑑 2 and 𝑠 = 𝑧𝑐 −

𝑑1 . Hence, 𝜃3 , is given by

The two solutions for 𝜃3 correspond

to the elbow-up and elbow-down
position.

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Articulated Configuration.
Similary 𝜃2 is given as

An example of an elbow manipulator

with offsets is the PUMA shown.
There are four solutions to the inverse
position kinematics left arm-elbow
up, left arm–elbow-down, right arm–
elbow up and right arm–elbow down.

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Spherical Configuration.
We next solve the inverse position The first joint variable is the base
kinematics for a three degree of rotation and a solution is given as
freedom spherical manipulator
shown next
Provided 𝑥𝑐 and 𝑦𝑐 are not both zero.
A second solution for 𝜃1 is given by

The angle 𝜃2 is given from the next

figure as

Where 𝑟 2 = 𝑥𝑐2 + 𝑦𝑐2 , 𝑠 = 𝑧𝑐 − 𝑑1 .

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Spherical Configuration.
The linear distance 𝑑3 is found as

The negative square root solution for 𝑑3 is disregarded and thus in this
case we obtain two solutions to the inverse position kinematics as long as
the wrist center does not intersect 𝑧0 .
If there is an offset then there will be left and right arm configurations as in
the case of the elbow manipulator.

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Articulated Manipulator with Spherical Wrist
The DH parameters for the frame The matrix 𝑅63 = 𝐴4 𝐴5 𝐴6 is given as
assignment are shown in the next
figure
𝑅63
𝑐4 𝑐5 𝑐6 − 𝑠4 𝑠6 −𝑐4 𝑐5 𝑠6 − 𝑠4 𝑐6 𝑐4 𝑠5
= 𝑠4 𝑐5 𝑐6 + 𝑐4 𝑠6 −𝑠4 𝑐5 𝑠6 + 𝑐4 𝑐6 𝑠4 𝑠5
−𝑠5 𝑐6 𝑠5 𝑠6 𝑐5

The equation to be solved for the final

The matrix 𝑅63 for the articulated or three variables is therefore
elbow manipulator

and the Euler angle solution can be

applied to this equation.

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Articulated Manipulator with Spherical Wrist
The three equations given by the If the positive square root is chosen in
third column in the above matrix the previous expressions, then 𝜃4 and
equation are given 𝜃6 are given by the Euler equations

Hence, if not both of first and The other solutions are obtained
second expressions are zero, we analogously. If 𝑠5 = 0, then joint axes
obtain 𝜃5 from 𝑧3 and 𝑧5 are collinear. This is a
singular configuration and only the
sum 𝜃4 + 𝜃6 can be determined.
One solution is to choose 𝜃4 arbitrarily
and then determine 𝜃6 Euler
Equations.
Robótica A
Dr. Mauro Eduardo Maya Méndez
 Example: Elbow manipulator
To summarize the geometric
approach for solving the inverse Given
kinematics equations, we write
give here one solution to the
inverse kinematics of the six
degree-of-freedom elbow
manipulator shown which has no
joint offsets and a spherical wrist.

Then with

𝑥𝑐 = 𝑜𝑥 − 𝑑6 𝑟13
𝑦𝑐 = 𝑜𝑦 − 𝑑6 𝑟23
𝑧𝑐 = 𝑜𝑧 − 𝑑6 𝑟33

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Example: Elbow manipulator

A set of DH joint variables is given by

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Summary

We studied the relationships between joint variables, 𝑞𝑖 and the position

and orientation of the end effector.
We may summarize the procedure based on the DH convention in the
following algorithm for deriving the forward kinematics for any
manipulator.
Step:
1. Locate and label the joint axes 𝑧0 , … , 𝑧𝑛−1 .
2. Establish the base frame. Set the origin anywhere on the 𝑧0 − 𝑎𝑥𝑖𝑠.
The 𝑥0 and 𝑦0 axes are chosen conveniently to form a right-handed
frame.
3. For 𝑖 = 1, … , 𝑛 − 1, perform Steps 3 to 5.
4. Locate the origin 𝑜𝑖 where the common normal to 𝑧𝑖 and
𝑧𝑖−1 intersects 𝑧𝑖 . If 𝑧𝑖 intersects 𝑧𝑖−1 locate oi at this intersection. If 𝑧𝑖
and 𝑧𝑖−1 are parallel, locate 𝑜𝑖 in any convenient position along 𝑧𝑖 .

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Summary

4. Establish 𝑥𝑖 along the common normal between 𝑧𝑖−1 and 𝑧𝑖 through

𝑜𝑖 , or in the direction normal to the 𝑧𝑖−1 − 𝑧𝑖 plane if 𝑧𝑖−1 and 𝑧𝑖
intersect.
5. Establish 𝑦𝑖 to complete a right-handed frame.
6. Establish the end-effector frame 𝑜𝑛 𝑥𝑛 𝑦𝑛 𝑧𝑛 . Assuming the 𝑛 − 𝑡ℎ joint
is revolute, set 𝑧𝑛 = 𝑎 along the direction 𝑧𝑖−1 . Establish the origin
on conveniently along 𝑧𝑛 , preferably at the center of the gripper or at
the tip of any tool that the manipulator may be carrying. Set 𝑦𝑛 = 𝑠 in
the direction of the gripper closure and set 𝑥𝑛 = 𝑛 as 𝑠 × 𝑎. If the tool
is not a simple gripper set 𝑥𝑛 and 𝑦𝑛 conveniently to form a right-
handed frame.

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Summary
7. Create a table of link parameters 𝑎𝑖 , 𝑑𝑖 , 𝛼𝑖 , 𝜃𝑖
𝑎𝑖 = distance along 𝑥𝑖 from 𝑜𝑖 to the intersection of the 𝑥𝑖 and
𝑧𝑖−1 axes.
𝑑𝑖 = distance along 𝑧𝑖−1 from 𝑜𝑖−1 to the intersection of the 𝑥𝑖
and 𝑧𝑖−1 axes. 𝑑𝑖 is variable if joint 𝑖 is prismatic.
𝛼𝑖 = the angle between 𝑧𝑖−1 and 𝑧𝑖 measured about 𝑥𝑖 .
𝜃𝑖 = the angle between 𝑥𝑖−1 and 𝑥𝑖 measured about 𝑧𝑖−1 . 𝜃𝑖 is
variable if joint 𝑖 is revolute.
8. Form the homogeneous transformation matrices Ai by substituting
the above parameters into (3.10).
9. Form 𝑇𝑛0 = 𝐴1 … 𝐴𝑛 . This then gives the position and orientation of
the tool frame expressed in base coordinates.

Robótica A
Dr. Mauro Eduardo Maya Méndez
 Summary
To control a manipulator, it is necessary to solve the inverse problem, i.e., given
a position and orientation for the end effector, solve for the corresponding set of
joint variable. We have considered the special case of manipulators for which
kinematic decoupling can be used (e.g., a manipulator with spherical wrist). The
determination of the inverse kinematics can be summarized by the following
algorithm
1. Find 𝑞1 , 𝑞2 , 𝑞3 such that the wrist center 𝑜𝑐 has coordinates given by

2. Using the joint variables determined in Step 1, evaluate 𝑅30 .

3. Find a set of Euler angles corresponding to the rotation matrix

We demonstrated a geometric approach for Step 1. In particular, to solve for

joint variable 𝑞𝑖 , we project the manipulator (including the wrist center) onto the
𝑥𝑖−1 − 𝑦𝑖−1 plane and use trigonometry to find 𝑞𝑖 .
Robótica A
Dr. Mauro Eduardo Maya Méndez
 Inverse kinematics

The equations in the preceding 1. In certain aplications, equations

example are, of course, much too must be solved at a rapid rate, say
difficult to solve directly in closed every 20 milliseconds.
form. This is the case for most 2. Having closed form solutions allows
robot arms. one to develop rules for choosing a
The inverse kinematics problem particular solution among several.
may or may not have a solution
For example, the motion of the revolute
and if it’s exists, it may or may not
joints may be restricted, so that not all
be unique.
mathematical solutions of the kinematic
The solutions may be difficult to equations will correspond to physically
obtain even when they exist. realizable configurations.
We are most interested in finding Once a solution to the mathematical
a closed form solution, rather than equations is identified, it must be further
a numerical solution. checked to see whether or not it
Closed form solutions are satisfies all constraints on the ranges of
preferable for two reasons: possible joint motions

Robótica A
Dr. Mauro Eduardo Maya Méndez