Kinematics of Robots

Alba Perez Gracia

c Draft date September 9, 2011
24
Chapter 2

Robot Kinematics Using Matrix

Algebra

2.1 Overview

In Chapter 1 we characterized the motion using vector and matrix algebra. In this chapter we
introduce the application of 4 × 4 homogeneous transforms in traditional robot analysis, where the
motion of a robot is described as a 4 × 4 homogoeneous matrix representing the position of the
end-effector. This matrix is constructed as the composition of a series of local displacements along
the chain.

In this chapter we will learn how to calculate the forward and inverse kinematics of serial robots
by using the Denavit-Hartenberg parameters to create the 4 × 4 homogeneous matrices.

2.2 Robot Kinematics

From a kinematics point of view, a robot can be defined as a mechanical system that can be
programmed to perform a number of tasks involving movement under automatic control. The main
characteristic of a robot is its capability of movement in a six-dimensional space that includes
translational and rotational coordinates.

We model any mechanical system, including robots, as a series of rigid links connected by joints.
The joints restrict the relative movement of adjacent links, and are generally powered and equipped
with systems to sense this movement.

25
26 CHAPTER 2. ROBOT KINEMATICS USING MATRIX ALGEBRA

The degrees of freedom of the robot, also called mobility, are defined as the number of independent
parameters needed to specify the positions of all members of the system relative to a base frame.
The most commonly used criteria to find the mobility of mechanisms is the Kutzbach-Gruebler
formula. For a robot with n links (counting the base) and j joints, in which each joint i allows fi
degrees of freedom, we compute the mobility as
j
X
M = 6(n − 1) − (6 − fi ). (2.1)
i=1

It is important to remark that this formula does not give the correct answer for several types of
closed, or parallel, robots. For a serial robot, the degrees of freedom are equal to the number of
joints multiplied by the mobility allowed by each joint.

The mobility indicates how many of the joints of the robot need to be actuated, also called active
joints. In a serial robot, all joints are active.

Once the mobility is defined and the active joints are identified, the two main problems of kinematic
analysis of robots are the direct kinematics and the inverse kinematics. In the direct kinematics,
also called forward kinematics, we define the position of the end-effector as a matrix which is a
function of the angles or slides at each joint. If the joint variables are known, the position of the
end-effector is completely specified. The inverse kinematics problem consists of finding the joint
angles to reach a given position. For serial robots, the inverse kinematics problem is harder than
the forward kinematics, while it is the opposite for parallel robots.

The solutions of the direct kinematics define the workspace of the robot. This can be defined
as the set of all possible positions of the end-effector, constructed using all possible values of the
joint variables within their range. Remember that position means location plus orientation; the
workspace of the robot is a six-dimensional subset of the six-dimensional space of rotations and
translations.

Because of the difficulty in visualizing the workspace, several subspaces have been defined [13].

• Reachable workspace (WR ): Set of all locations of the origin of the end-effector frame that
the robot can reach. It is a three-dimensional subset of the workspace.

• Dextrous workspace (WD ): Set of all locations of the origin of the end-effector frame that the
robot can reach with any orientation. It is useful because the robot has full dexterity in this
subspace, which ensures that any task can be performed within it.

• Workspace with constant orientation (Wθ ): Set of all locations of the origin of the end-effector
frame that the robot can reach with a specified orientation for the end-effector.
2.3. BASIC JOINTS 27

Notice that
[ \
Wθ = WR , Wθ = WD (2.2)
θ∈range θ∈range

Figure 2.1 shows the typical sketch of the reachable workspace for a commercial robot.

Figure 2.1: Reachable workspace, Adept Viper m650 robot (Adept Technologies, Inc.)

2.3 Basic joints

Releaux [19] identified six basic types of joints that continuously share a surface of contact, that he
called lower pairs. His classification is still used today in most robotics and mechanisms books. He
defined revolute (R), cylindric (C), prismatic (P), spherical (S), helical (H) and plane (E) joints.
In our research, we only consider the basic types of revolute and prismatic joints; any of the others
can be obtained by combining the two basic types with certain additional constraints. In addition
to these, we refer to the universal joint (T), which can be modeled as two revolute joints at right
angles. See Figure 2.2.

The revolute joint is a one-degree-of-freedom joint that allows a rotation of angle θ about an
arbitrary located joint axis G = g + g0 . The prismatic joint is also a one-degree-of-freedom joint,
which allows translation along the direction h of the axis H. It is interesting to notice that the
location of the prismatic axis does not matter for the movement of the prismatic joint. The cylindric
joint is a general screw motion, in which the rotation about and the translation along the screw
axis S are nonzero and independent. It can be constructed as the composition of a revolute joint
28 CHAPTER 2. ROBOT KINEMATICS USING MATRIX ALGEBRA

Figure 2.2: Common types of joints

and a prismatic joint with same direction. In a similar fashion, the universal (T) joint and the
spherical (S) joint can be constructed from individual rotations.

2.4 Forward Kinematics Equations

Following the traditional matrix kinematics equations, we will represent local transformations from
joint to joint using 4 × 4 homogeneous matrices. The composition of these displacements will give
as a result the displacement from the base to the end-effector.

Angles will be measured usually taking the ”extended” position as the origin. However, this is an
arbitrary choice and any reference configuration can be used as the zero for the joint variables.

In order to create the local transformations along the chain, we identify the lines locating the
joint axes, Si , and we compute the common normal lines Ai,i+1 between joint Si and joint Si+1 .
We consider that the link joining two consecutive joints extends along the common normal line,
regardless of its real shape.

We assign local coordinate frames with the Z axis along the joint axis and the X axis along the com-
mon normal line. Local displacements will take us from local coordinate frame to local coordinate
frame, from the base to the end-effector, see Figure 2.3.

2.4.1 Denavit-Hartenberg parameters

In 1955, Denavit and Hartenberg introduced the methodology of 4 × 4 homogeneous matrix trans-
formations to analyze robotic systems, which became a standard tool in robotics. They shaped
the local transformations along the chain as screw displacements about the Z and X axes, de-
pendent on only four parameters. With the pass of the years, slightly different versions of the
Denavit-Hartenberg parameters have been developed. Our notation is similar to Craig [11].
2.4. FORWARD KINEMATICS EQUATIONS 29

Figure 2.3: Local transformations along the links of a robot.

We locate the first local frame with the Z axis along the first joint axis S1 and the X axis along
the common normal A12 ; the origin of the frame is located at the intersection of these two lines.
Assume that a general displacement [G] locates this first local frame with respect to the fixed frame,
as shown in Figure 2.3.

The second local frame has the same X axis but now the Z axis is aligned with S2 , with its origin at
the intersection point of these two lines, while the third local frame has same Z axis as the second
and the line A23 as its X axis. From the local frame attached to the last joint axis to the moving
frame attached to the gripper we define a general displacement [H].

Only four parameters are needed to transform from local frame to local frame: two correspond to
the transformation along the link, which is an X-screw displacement, and two more to perform a
Z-screw displacement about the joint. These parameters are:

• Twist angle αi−1,i : Angle between joint axes Si−1 and Si measured about the common normal
line Ai−1,i .

• Link length ai−1,i : Distance between joint axes Si−1 and Si measured along the common
normal line Ai−1,i .

• Joint angle θi : Angle between previous common normal line Ai−1,i and next common normal
line Ai,i+1 , measured about joint axes Si . Notice that if joint Si is a revolute joint, the joint
angle will include the variable value of the joint rotation.
30 CHAPTER 2. ROBOT KINEMATICS USING MATRIX ALGEBRA

Joint αi−1,i ai−1,i di θi

1 0 0 d1 θ1
2 α12 a12 d2 θ2
3 α23 a23 d3 θ3

Table 2.1: Denavit-Hartenberg parameters.

• Offset di : Distance between previous common normal line Ai−1,i and next common normal
line Ai,i+1 , measured along joint axes Si . Notice that if joint Si is a prismatic joint, the offset
will include the variable value of the slide.

The Denavit-Hartenberg parameters describe in a simple way the geometry of the robot. They are
usually arranged on a table. For the robot of Figure 2.3 we create the corresponding Table 2.1,
where we have not specified whether the joints are revolute or prismatic.

The D-H parameters are used to create the screw displacements,

   
1 0 0 ai−1,i cos θi − sin θi 0 0
   
0 cos αi−1,i − sin αi−1,i 0   sin θi cos θi 0 0
[X(αi−1,i , ai−1,i )] = 
0 sin α
 [Z(θi , di )] =  .
 i−1,i cos αi−1,i 0  
 0
 0 1 di 

0 0 0 1 0 0 0 1
(2.3)

2.4.2 Kinematics Equations

The local screw displacements created using the D-H parameters, together with the initial and final
displacements [G] and [H], are multiplied together to obtain the displacement from the fixed frame
to the frame attached to the end-effector,

[D] = [G][X(α01 , a01 )][Z(θ1 , d1 )][X(α12 , a12 )][Z(θ2 , d2 )] . . . [X(αn−1,n , an−1,n )][Z(θn , dn )][H].
(2.4)

The 4 × 4 transform [D] is called the forward kinematics or simply the kinematics equations. It
provides with all possible positions of the end-effector as a function of the joint variables. The
kinematics equations solve the direct kinematics problem; for a given set of joint variables, the
position of the end-effector is immediately calculated.
2.5. FORWARD KINEMATICS EXAMPLE 31

2.5 Forward Kinematics Example

Let us apply the technique described above to calculate the forward kinematics equations of the
robot presented in Figure 2.4.

S1(q1)

{F} {M}

S2(q2) S3(q3)

Figure 2.4: Example: a three-jointed serial robot.

The transformation from the fixed frame to the first local frame is a translation along the Z axis,
given by the matrix
 
1 0 0 0
 
0 1 0 0
[G] = 
0
 (2.5)
 0 1 80

0 0 0 1

The local transformations from the first joint axis to the last can be calculated using the Denavit-
Hartenberg parameters of the robot, presented in Table 2.2.

Joint αi−1,i ai−1,i di θi

1 0 0 0 θ1
2 π/2 70 0 θ2
3 0 65 0 θ3

Table 2.2: Denavit-Hartenberg parameters for the robot of Figure 2.4.

32 CHAPTER 2. ROBOT KINEMATICS USING MATRIX ALGEBRA

The screw displacements about the X axis and Z axis are created using the rows of Table 2.2,
   
cos θ1 − sin θ1 0 0 1 0 0 70
   
 , [X12 ] = [X(π/2, 70)] = 0 0 −1 0  ,
 sin θ1 cos θ1 0 0  
[Z1 ] = [Z(θ1 , 0)] = 
 0
 0 1 0
0 1 0
 0
0 0 0 1 0 0 0 1
   
cos θ2 − sin θ2 0 0 1 0 0 65
   
 sin θ2 cos θ2 0 0 0 1 0 0 
[Z2 ] = [Z(θ2 , 0)] = 
 0
 , [X23 ] = [X(0, 65)] = 
0 0 1 0  ,

 0 1 0  
0 0 0 1 0 0 0 1
 
cos θ3 − sin θ3 0 0
 
 sin θ3 cos θ3 0 0
[Z3 ] = [Z(θ3 , 0)] = 
 0
. (2.6)
 0 1 0
0 0 0 1

The last local displacement, from the third joint axis to the end-effector frame, is calculated to be
 
1 0 0 35
 
0 0 1 −15
[H] =  . (2.7)
0 −1 0 0 

0 0 0 1

The kinematics equations are constructed by multiplying the local transformations,

[D] = [G][Z1 ][X12 ][Z2 ][X23 ][Z3 ][H] =

 
cos θ1 cos(θ2 + θ3 ) − sin θ1 − cos θ1 sin(θ2 + θ3 ) 5 cos θ1 (14 + 13 cos θ2 + 7 cos(θ2 + θ3 ) + 3 sin(θ2 + θ3 )
 sin θ cos(θ + θ ) cos θ
 1 2 3 1 − sin θ1 sin(θ2 + θ3 ) 5 sin θ1 (14 + 13 cos θ2 + 7 cos(θ2 + θ3 ) + 3 sin(θ2 + θ3 ) 

80 − 15 cos(θ2 + θ3 ) + 65 sin θ2 + 35 sin(θ2 + θ3 ) 
 
 sin(θ2 + θ3 ) 0 cos(θ2 + θ3 )
0 0 0 1
(2.8)

2.6 Inverse Kinematics

The inverse kinematics consists of finding expressions for the joint variables as a function of the
desired position of the end-effector. Given the matrix formulation for the kinematics equations,
usually these formulas are obtained by equating elements of the matrix to the elements of the
desired task position and manipulating the expressions. For a good collection of results, see [24].

In general, for a robot with n joints and joint variables qi , i = 1 . . . n, we state the inverse kinematics
2.7. INVERSE KINEMATICS EXAMPLE 33

problem by equating the kinematics equations [D] to a desired position of the end-effector [P ],

[D(q1 , q2 , . . . qn )] = [P ]. (2.9)

There is no general formulation to solve for the joint variables in this equation; each case presents
different techniques to isolate the joint variables qi .

Most of the times we obtain several solutions. For robots with more than six degrees of freedom,
and for some lower-dof robots, we may obtain infinite solutions, and additional strategies must be
used to decide the best set of joint variables.

2.7 Inverse Kinematics Example

To illustrate the somewhat vague description of previous section, we will solve the inverse kinematics
for the example presented in Section 2.5.

Take the matrix from Eq.(2.8) and equate it to a desired task position [P ] = [pij ]. Notice that we
can solve for θ1 directly in elements (1, 2) and (2, 2); we obtain

θ1 = arctan(−p12 , p22 ). (2.10)

Next we can solve for the sum of the angles θ2 + θ3 for instance using elements (3, 1) and (3, 3).
Again we obtain
θ2 + θ3 = arctan(p31 , p33 ). (2.11)

Finally, we can solve for θ2 in elements (2, 4) and (3, 4),

1 1 p24
θ2 = arctan( (p34 − 80 + 15p33 − 35p31 ), − ( + 14 + 7p33 − 3p31 )), (2.12)
65 13 5p12
and θ3 = (θ2 + θ3 ) − θ2 . In this case there are no multiple solutions and the solution is fairly easy
to obtain.
34 CHAPTER 2. ROBOT KINEMATICS USING MATRIX ALGEBRA

Homework 2

Chapter 2

Use Maple to create the forward kinematics equations of the robot shown in Figure 2.5. Take the
given configuration of the robot as the reference configuration.

Figure 2.5: Welding Robot (Fanuc Robotics)

1. Identify the joint axes and the common normal lines on the drawing or sketch.

2. Write the Plücker coordinates of the joint axes.

3. Create the D-H table.

4. Indicate the location of the world frame and the end-effector frame and construct the first
and last transformations [G] and [H].
2.7. INVERSE KINEMATICS EXAMPLE 35

5. Write the matrices of the local transformations and multiply them together to obtain the
forward kinematics matrix.

6. What are the coordinates of the origin of the end-effector frame as a function of the joint
variables?

7. Draw the trajectory of the origin of the end-effector frame for the following sequence of joint
values: θ1 from 0o to 90o , θ2 , θ3 from 0o to 45o , θ4 , θ5 and θ6 from 0o to 180o .

8. (Optional): Create a simulation of the robot using Maple.

Bibliography

[1] Ablamowicz, R. and Sobczyk, G., Lectures on Clifford (Geometric) Algebras and Applicatons,
Birkhauser, Boston 2004.

[2] Artin, M., Algebra, Prentice-Hall, New Jersey, 1991.

[3] Bayro Corrochano, E., Daniilidis, K., and Sommer, G., “Motor Algebra for 3D Kinematics:
The Case of Hand-Eye Calibration”, Journal of Mathematical Imaging and Vision, 13:79-100,
2000.

[4] Bayro Corrochano, E., Geometric Computing for Perception Action Systems, Springer-Verlag,
New York, 2001.

[5] Bayro Corrochano, E., Handbook of Geometric Computing, Springer-Verlag, Berlin, 2005.

[6] Bayro-Corrochano, E., and Zamora-Esquivel, J., “Differential and Inverse Kinematics of Robot
Devices Using Conformal Geometric Algebra”, Robotica, 25:43-61, 2007.

[7] Bottema, O., and Roth, B., Theoretical Kinematics, North Holland Press, NY, 1979.

[8] Browne, J., Grassmann Algebra: Exploring applications of extended vector algebra with
Mathematica, Swinburne University of Technology, Melbourne, Australia, 2001. (Draft from
http://www.ses.swin.edu.au/homes/browne/grassmannalgebra/book/index.htm).

[9] Cohn, P.M., Algebra, 2nd. edition, John Wiley & Sons, 1989.

[10] Ge, Q.J., 2002, “Projective convexity in computational kinematic geometry”, Proceedings of
the ASME Design Engineering Technical Conference, pages 715-723, Montreal, Canada, 2002.

[11] Craig, J. J., Introduction to Robotics, Mechanics and Control, Addison Wesley Publ. Co., 1989.

[12] Gosselin, C.M., St-Pierre, E. and Gagne, M., 1996, “On the development of the agile eye: me-
chanical design, control issues and experimentation”, IEEE Robotics and Automation Society
Magazine, 3(4):29-37.

[13] Kumar, A., and Waldron, K.J., 1981, “The Workspace of a Mechanical Manipulator,” ASME
J. Mech. Design, 103: 665-672.

73
74 BIBLIOGRAPHY

[14] McCarthy, j.m., Introduction to Theoretical Kinematics, The MIT Press, 1990.

[15] McCarthy, J.M., Geometric Design of Linkages, Springer, New York 2000.

[16] Murray, R., Li, Z. and Sastry, S., A Mathematical Introduction to Robotic Manipulation, CRC
Press, Boca Raton, 1994.

[17] Perez, A. and McCarthy, J. M., 2005, “Clifford Algebra Exponentials and Planar Linkage
Synthesis Equations”,ASME Journal of Mechanical Design, 127(5): 931-940.

[18] Porteous, I. R.,Clifford Algebras and the Classical Groups, Cambridge Studies on Advanced
Mathematics, 50, Cambridge University Press, Cambridge, 1995.

[19] Releaux, F., 1875, The Kinematics of Machinery: Outlines of a Theory of Machines, Dover
Publications, New York, translation of 1963.

[20] Selig, J.M., Geometrical Methods in Robotics, Springer, London 1996.

[21] Selig, J.M., Geometric Fundamentals of Robotics, Springer, London, 2005.

[22] Sommer, G., Geometric Computing with Clifford Algebras, Chapter1, Springer, Berlin, 2001.

[23] Sugimoto, K., 1987, “Kinematic and Dynamic Analysis of Parallel Manipulators by Means of
Motor Algebra”, ASME Journal of Mechanisms, Transmissions and Automation in Design,
109(1):3-5.

[24] Tsai, L. W., 1999, Robot Analysis: The Mechanics of Serial and Parallel Manipulators, John
Wiley and Sons, New York, NY.