Technical report from Automatic Control at Linköpings universitet

On robot modelling using Maple

Johanna Wallén
Division of Automatic Control
E-mail: johanna@[Link]

9th August 2007

Report no.: LiTH-ISY-R-2723

Address:
Department of Electrical Engineering
Linköpings universitet
SE-581 83 Linköping, Sweden

WWW: [Link]

AUTOMATIC CONTROL
REGLERTEKNIK
LINKÖPINGS UNIVERSITET

Technical reports from the Automatic Control group in Linköping are available from
[Link]
 Abstract
This report studies robot modelling by means of the computer algebra tool
Maple. First coordinate systems are described, and the more general way
with transformation matrices is chosen in the further work. The position
kinematics of the robot are then described by homogeneous transformations.
The Denavit-Hartenberg representation is used, which is a systematic way
to develop the forward kinematics for rigid robots. The velocity kinematics
is then described by the Jacobian. The industrial robot IRB1400 from ABB
Robotics is used as an example of the theory and to show the use of the
procedures developed in the Maple programming language.

Keywords: Maple, industrial robot, homogeneous transformation,

Denavit-Hartenberg representation, forward kinematics, Jabcobian
 Avdelning, Institution Datum
Division, Department Date

Division of Automatic Control

2007-08-09
Department of Electrical Engineering

Språk Rapporttyp ISBN

Language Report category —
 Svenska/Swedish  Licentiatavhandling ISRN
 Engelska/English
  Examensarbete —
 C-uppsats
Serietitel och serienummer ISSN
 D-uppsats Title of series, numbering 1400-3902
  Övrig rapport



URL för elektronisk version

LiTH-ISY-R-2723
[Link]

Titel On robot modelling using Maple

Title

Författare Johanna Wallén

Author

Sammanfattning
Abstract

This report studies robot modelling by means of the computer algebra tool Maple. First
coordinate systems are described, and the more general way with transformation matrices
is chosen in the further work. The position kinematics of the robot are then described by
homogeneous transformations. The Denavit-Hartenberg representation is used, which is a
systematic way to develop the forward kinematics for rigid robots. The velocity kinematics
is then described by the Jacobian. The industrial robot IRB1400 from ABB Robotics is used
as an example of the theory and to show the use of the procedures developed in the Maple
programming language.

Nyckelord
Keywords Maple, industrial robot, homogeneous transformation, Denavit-Hartenberg representation,
forward kinematics, Jabcobian
Contents
1 Introduction 4

2 Related research 4
2.1 The chosen models . . . . . . . . . . . . . . . . . . . . . . . . . . 4
2.2 Why Maple? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

3 Basics about robots and coordinate systems 5

3.1 Common parts of the industrial robot . . . . . . . . . . . . . . . 5
3.2 Pairs and joints . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
3.3 Coordinate systems . . . . . . . . . . . . . . . . . . . . . . . . . . 6

4 Position kinematics 8
4.1 Translation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
4.2 Rotation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
4.3 Different representations of rotations . . . . . . . . . . . . . . . . 10
4.4 Rigid motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
4.5 Differentiation of the transformation matrix . . . . . . . . . . . . 12

5 Denavit-Hartenberg representation 13
5.1 Homogeneous matrix . . . . . . . . . . . . . . . . . . . . . . . . . 13
5.2 Standard Denavit-Hartenberg representation . . . . . . . . . . . 14

6 D-H representation – a robot example 15

6.1 Link parameters and joint variables . . . . . . . . . . . . . . . . . 16
6.1.1 Configuration of IRB1400 . . . . . . . . . . . . . . . . . . 17
6.1.2 Transformed joint variables . . . . . . . . . . . . . . . . . 17
6.1.3 Mathematical summary . . . . . . . . . . . . . . . . . . . 18
6.1.4 Thoughts about the closed kinematic chain . . . . . . . . 18
6.1.5 How to do in Maple . . . . . . . . . . . . . . . . . . . . . 19
6.2 Transformation matrices . . . . . . . . . . . . . . . . . . . . . . . 23
6.2.1 How to do in Maple . . . . . . . . . . . . . . . . . . . . . 25
6.3 Rigid motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
6.3.1 How to do in Maple . . . . . . . . . . . . . . . . . . . . . 27

7 Velocity kinematics 27
7.1 Manipulator Jacobian . . . . . . . . . . . . . . . . . . . . . . . . 28
7.2 Linear velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
7.2.1 How to do in Maple . . . . . . . . . . . . . . . . . . . . . 30
7.3 Angular velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
7.3.1 How to do in Maple . . . . . . . . . . . . . . . . . . . . . 32
7.4 Resulting Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . 33
7.4.1 How to do in Maple . . . . . . . . . . . . . . . . . . . . . 34
7.5 Discussion about the Jacobian . . . . . . . . . . . . . . . . . . . 34
7.5.1 Interpretation of the Jacobian . . . . . . . . . . . . . . . . 35
7.5.2 Singularities . . . . . . . . . . . . . . . . . . . . . . . . . . 35

8 Acceleration equations 36
9 Summary of example IRB1400 in Maple 37
9.1 Step by step . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

10 Concluding remarks and future work 39

A Malpe implementation 41
A.1 [Link] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
A.2 type . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
A.3 DHparameter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
A.4 DHlist2link . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
A.5 DHvariablechange . . . . . . . . . . . . . . . . . . . . . . . . . . 44
A.6 DHlink . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
A.7 DHrobot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
A.8 DHmatrixA . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
A.9 DHmatrixT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
A.10 DHpositionT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
A.11 DHrotationT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
A.12 DHtransformationT . . . . . . . . . . . . . . . . . . . . . . . . . 49
A.13 DHlinearjacobian . . . . . . . . . . . . . . . . . . . . . . . . . . 50
A.14 DHangularjacobian . . . . . . . . . . . . . . . . . . . . . . . . . 51
A.15 DHjacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

B Malpe implementation, another way to do it 54

B.1 A more direct approach . . . . . . . . . . . . . . . . . . . . . . . 54

3
1 Introduction
In this report a first step towards making a toolbox for industrial robot modelling
in the computer algebra tool Maple [13] is taken. The aim with this project
is not to build a comprehensive and complex toolbox, but to study relatively
simple robot models in order to understand the techniques.
The work leads towards a modelling platform for efficient derivation of the
necessary equations that in the future can aid e.g., developing different model
representations of robot structures with or without mechanical flexibilities or
performing sensor fusion. Additional information from sensors demands signal
processing and sensor fusion, which is hard to achieve without good models and
good modelling tools. The idea is to provide a platform where it is simpler to
evaluate different kinds of sensors and also various sensor locations using Maple
as a tool to symbolically generate the kinematic models. Thereafter the models
will be incorporated in Matlab- or C-code to implement, e.g., state estimation
using an Extended Kalman Filter (EKF) algorithm.
The numerical calculations described in this report can also be made by
means of Robotic Toolbox in Matlab [4], but the symbolic manipulations can-
not be performed in Robotics Toolbox.

2 Related research
An example of previous work in this area is [3], where a Mathematica based
approach to robot modelling is presented by Bienkowski and Kozlowski. The
kinematic derivation in [3] does, however, not use the Denavit-Hartenberg (D-H)
representation, which is a standard in the robotics field (see Section 5 for a
description). A fairly well-known contribution is Robotics Toolbox [4], developed
by Corke, where numerically calculations of kinematics, dynamics and trajectory
planning are possible. It provides simulation and also analysis of results from
real robot experiments. Another contribution by Corke is [6], but the focus there
is on the dynamics and how dynamic models of robots can be simplified using
Maple. See also [5] for the details. A Mathematica package, Robotica [15],
has also been developed by Nethery ans Spong. This package is based upon [17]
and gives support for both kinematic as well as dynamic modelling. Although
similar to what in presented in this report, the package Robotica is no longer
supported or updated and it is not sure that it works in the last version of
Mathematica without editing the source code (according to the homepage of
Mark W. Spong, one of the authors of Robotica).

2.1 The chosen models

The work in this report can be done for all axis and the model is without joint
flexibilities. The examples in this report show the results for just the two or
three first axis of the robot model IRB1400 [16] from ABB Robotics, since it is
hard to take in the whole picture because of the size of the expressions for all
six axis.

4
2.2 Why Maple?
There are many computer algebra tools like, e.g., Matlab Symbolic Toolbox,
Mathematica and Maple. They all have different benefits and disadvantages.
The reason why Maple is chosen in this case is that ABB uses this tool and it
is a large benefit to be able to deeper understand the work done at ABB.
The work in this report is performed in Maple 10 and 11 and in this report
the implemented toolbox is called DH-toolbox.

3 Basics about robots and coordinate systems

It is needed to define suitable coordinate systems for the following work, but
first of all it is necessary to explain common terms for the different parts used
in the industrial robot modelling.

3.1 Common parts of the industrial robot

The individual bodies that together form a robot are called members or links.
Generally a robot with n degrees-of-freedom has n + 1 links and the base of the
robot is defined as link 0. It also has n joints. Most of the industrial robots
have six degrees of freedom, which makes it possible to control both the position
of the tool and its orientation. A systematic way to calculate the number of
degrees of freedom of a robot by means of the so-called Grübler or Kutzbach
criterion is shown in [18].
The dynamics of the robot is coupled and therefore movements of one joint
will also affect the other joints. For example link 2 attaches joint 2 to joint 3,
i.e., a joint provides some physical constraints on the relative motion between
the two links. Coordinate system number 3 is placed in the end of link 3, i.e.,
in joint 3, so the coordinates of each point of link 3 are constant in coordiante
frame 3 for every movement of the robot links. See for example Figure 3-1, page
64, in [17] for an illustration of these notions.
The assumption of all links as rigid bodies makes the study of robots much
easier to understand. For high-speed robots, there are significant elastic effects,
and these should be taken into consideration. Non-rigid bodies, e.g., chains,
cables or belts, may also be seen as links, because they momentarily act as rigid
bodies. Also two links that are connected to each other in such a way that no
relative motion can occur between them can, from a kinematic point of wiev,
be considered as a single link. [18]

3.2 Pairs and joints

The ith joint has a variable, qi , that represents the relative displacement between
adjacent links, i.e., each joint has one degree of freedom. The total degrees of
freedom for the robot is however equal to the degrees of freedom associated with
the moving links minus the number of contraints imposed by the joints. [18]
The kind of relative motion between the links, connected by the joint, is
determined by the contact surfaces (called pair elements) between the links.
Two pair elements form a kinematic pair, and if the two links are in contact with
each other by a substantial surface area it is called a lower pair. Otherwise, if the
links are in contact along a line or at a point, it is called a higher pair. Examples

5
of lower pairs are revolute joint (also called turning pair), prismatic joint (called
sliding pair), cylindrical joint, helical joint (called screw pair), spherical joint
and plane pair. The frequently used universal joint is a combination of two
intersecting revolute joints. Higher pairs are gear pair and cam pair. More about
these joints and pairs and their corresponding number of degrees of freedom can
be found in [18]. In this report it is sufficient to know that a revolute joint has
a cylindrical shape where the motion is a rotation with angle θ. On the other
hand, the prismatic joint can be described by a cube with side d and its motion
is a translation along the defined z-axis.
The forward kinematics determines the pose, i.e., position and orientation,
of the robot tool when the joint variables qi of the robot are given. For a revolute
or rotational joint, the variable qi is the angle of rotation between the links. In
the case of prismatic or sliding joint, the variable is the link extension. This is
described in more detail in Section 5.2.
The joint variables q1 , . . . , qn form a set of generalised coordinates for an
n-link robot. Figure 1 shows the joint variables qi for IRB1400 from ABB
Robotics, which only has revolute joints. [4, 17]

q3
q4

q6

q5

q2

q1

Figure 1: The robot IRB1400ABB from ABB Robotics with joint variables qi .

According to [8, 17] all types of joints can be described by means of revolute
or prismatic joints (both having one degree of freedom), and further work is
therefore only applied to revolute or prismatic joints.

3.3 Coordinate systems

The simplified robot model presented in Figure 2 with two arms in the ground
plane has no gravitational force. One way to represent the two axis is to have a
coordinate system with the unit vectors r̂1 and θ̂1 for the first arm and the unit
vectors r̂2 and θ̂2 for the following arm [2]

6
 r̂2
ŷ
θ̂2

θ̂1

r̂1
q2

q1
x̂

Figure 2: Simplified robot model with two arms in the ground plane. The
figure comes from [2] and shows the global coordinates x̂ and ŷ and the local
coordinate systems with the unit vectors r̂i , θ̂i associated with the two arms.

r̂1 = cos(q1 )x̂ + sin(q1 )ŷ (1)

θ̂1 = − sin(q1 )x̂ + cos(q1 )ŷ (2)
r̂2 = cos(q2 )r̂1 + sin(q2 )θ̂1 (3)
θ̂2 = − sin(q2 )r̂1 + cos(q2 )θ̂1 (4)

Neccessary information about the orientation is actually covered by the unit

vectors r̂1 and r̂2 . The unit vectors θ̂1 and θ̂2 can for example be seen as a
rotation by an angle π2 from the r̂1 - and r̂2 -vectors. With this in mind it is
easier to understand and verify the derivatives of these unit vectors below

r̂˙1 = q̇1 θ̂1 (5)

˙
θ̂1 = −q̇1 r̂1 (6)
r̂˙2 = (q̇1 + q̇2 )θ̂2 (7)
˙
θ̂2 = −(q̇1 + q̇2 )r̂2 (8)

When r̂1 is differentiated it is only an angular change, because the length of

the vector r̂1 (represented by r in the r̂1 -direction) is a constant. This results in
r̂˙1 in only the θ̂1 -direction with the lengt q̇1 coming from the inner derivative.
It is also verified from calculations in [2]. Same discussion can be used when
differentiating θ̂1 , now with the outlook that θ̂1 is rotated by an angle π2 from
˙
r̂1 . It can now easily be seen that θ̂1 is in the −r̂1 -direction with length q̇1 from
the inner derivative.

7
 Then the derivatives of the unit vectors for the second axis are derived. r̂˙2
is in the θ̂2 -direction with the length q̇1 + q̇2 coming from the inner derivative.
˙
θ̂2 is also directed orthogonal to r̂2 , i.e., in the −r̂2 -direction with the length
from the inner derivative.
The second derivates of the unit vectors are derived in [2] and can be depicted
as follows

r̂¨1 = q̈1 θ̂1 − q̇12 r̂1 (9)

¨
θ̂1 = −q̈1 r̂1 − q̇12 θ̂1 (10)
r̂¨2 = −(q̇1 + q̇2 )2 r̂2 + (q̈1 + q̈2 )θ̂2 (11)
¨
θ̂2 = −(q̈1 + q̈2 )r̂2 − (q̇1 + q̇2 )2 θ̂2 (12)

By means of intuition the equations above can be verified. Study for exam-
ple (9). It is a derivative of the product r̂˙1 = q̇1 θ̂1 , so the first term q̈1 θ̂1 is in
the θ̂1 -direction because only q̇1 is differentiated and θ̂1 is constant. The second
term with length q̇12 from the inner derivative of θ̂1 is in the r̂1 -direction as
expected, because it is a derivative of θ̂1 . The other derivatives can be verified
intuitively following a similar discussion.
In this section only the first two axes are described by these coordinate
systems, but it is straight forward to move on and describe the other axes in
a similar manner. The trick is to see that all necessary information about the
orientation of the robot arm is collected in (1) as described above, because (2)
is just (1) rotated by the angle π2 . The derivatives were however more difficult
to perform in Maple than expected. It is also more specific than the general
homogeneous transformations, where the rotations and transformations between
the coordinate systems are represented by a single matrix. These homogenous
transformations are therefore used in the further work.

4 Position kinematics
Kinematics describes the movements of bodies without considerations of the
cause. Position, velocity, acceleration and higher derivatives of the position
variables are studied. Robot kinematics especially studies how various links
move with respect to each other and with time [4]. Dynamics on the other hand
describes the movements with respect to also the forces that caused the motion.
There are two ways to describe the position kinematics. Forward kinematics
is one way where the idea of kinematic chains is used and the position and
orientation of the robot tool are determined in terms of the joint variables.
Kinematic chains means that the robot is seen as a set of rigid links connected
together by different joints. In this report the forward kinematics is studied,
and the work is performed with homogeneous transformations (see Section 4.4)
and the Denavit-Hartenberg (D-H) representation (see Section 5). The other
way is inverse kinematics which means to calculate the joint configuration of
the robot from the position and orientation of the robot tool. This is in general
a much more difficult problem to solve than forward kinematics. [16]
If nothing is mentioned, the source used in this section is [17].

8
4.1 Translation
First a parallel translation of the vector p1 by the vector d10 in coordinate frame 0
is described. It can shortly be written as

p0 = p1 + d10 (13)

4.2 Rotation
The transformation between coordinate frame 0 and frame 1 is described and
then frame 2 is added to show the structure of the combined transformations
even more.
The basic rotation matrix R10 describes the transformation of the vector p
between frame 0 and frame 1 as

p0 = R01 p1 (14)

The matrix R01 is built upon scalar products between the orthonormal coordi-
nate frames consisting of the standard orthonormal unit vectors {x0 , y0 , z0 } and
{x1 , y1 , z1 } in frame 0 and frame 1 respectively. R01 can thus be given by
 
x1 · x0 y1 · x0 z1 · x0
R01 = 
 
 x1 · y0 y1 · y0 z1 · y0 
 (15)
x1 · z0 y 1 · z0 z1 · z0

For example a rotation with an angle θ around the z-axis in the base frame can
be described with the basic rotation matrix
 
cos θ − sin θ 0
R01 = Rz,θ = 
 
 sin θ cos θ 0  (16)
0 0 1

Since the scalar product is commutative, the matrix is orthogonal and the inverse
transformation p1 = R10 p0 is given by

R10 = (R01 )−1 = (R01 )T (17)

Now coordinate frame 2 is added, which is related to the previous frame 0

and frame 1 as follows.

p0 = R01 p1 (18)
p0 = R02 p2 (19)
p1 = R12 p2 (20)

Combining the rotation matrices gives the transformation from frame 2 to

frame 0 according to
p0 = R01 R12 p2 = R02 p2 (21)
The transformation matrices describe the order of these rotations. First a ro-
tation of the frame 1 is performed relative to frame 0 described by R01 followed
by rotating the frame 2 relative to frame 1 according to R12 . The order of the

9
transformation matrices cannot be changed, because then the interpretation
and the result of these rotations are quite different. Rotation is unlike trans-
lation a matrix quantity and therefore the rotational transformations do not
commute. [17]
Above the rotations are made around successive different frames, but some-
times it is more desirable to rotate around the fixed frame 0 all the time. This
is performed by multiplying the transformation matrices in the reverse order
compared to (21), giving the rotation matrix

R02 = R12 R01 (22)

4.3 Different representations of rotations

The elements in the rotation matrix R are not independent. A rigid body can
at most have three rotational degrees of freedom, and thus at most three in-
dependent variables are needed to specify the orientation. There are different
representations, each of them describing an arbitrary rotation by three indepen-
dent quatities, except the quaternion representation which uses four quantities.

Axis/angle The axis/angle representation is based on the idea that a rotation

matrix always can be represented by a single rotation about a single axis in space
by a suitable angle. It can be described by the relation

R = Rk,θ (23)

where k is the unit vector that defines the axis of rotation and θ is the angle
of rotation about the axis. The equation is therefore called the axis/angle
representation of R. The representation is however not unique, since a rotation
by the angle −θ about −k gives the same result.

Euler angles The method Euler angles is a more common way to specify the
rotation matrix R. The Euler angles θ, φ and Ψ are obtained by the following
successive rotations. For example the orientation of frame 1 relative to frame 0
are specified by first rotating about the z0 -axis by the angle φ. Then rotate
about the current y-axis by the angle θ and last but not least rotate about
the current z-axis (which now is the z1 -axis) by the angle Ψ. The resulting
rotational transformation R01 can be described by the product

R01 = Rz0,φ Ry,θ Rz,Ψ (24)

If it is preferred to see the rotations described in a figure, view Figure 2-7 in [17].

Roll-pitch-yaw angles Here the rotation matrix R is described as a product

of successive rotations about the principal coordinate axes x0 , y0 and z0 in a
specified order. The roll ψ, pitch θ and yaw Ψ angles can be seen in Figure 2-8
in [17]. First yaw about x0 by the angle Ψ, then pitch about y0 by the angle θ
and last roll about z0 by the angle φ. This is described by the matrix product

R01 = Rz0,ψ Ry0,θ Rx0,Ψ (25)

10
Quaternions Quaternions are an extension to complex numbers and a quater-
nion consists of four elements q1 . . . q4 . It can mathematically be written

q = q1 + iq2 + jq3 + kq4 (26)

where i, j and k are mutually orthogonal imaginary units with special properties.
A quaternion can also be described as a tuple, but that would lead too far in
this very short description of quaternions.
They can be used to compactly describe an arbitrary rotation. Two vectors
p0 and p1 are separated by the angle θ, i.e., there is a homogeneous transfor-
mation
p0 = Rotk,θ (27)
The rotation is performed around the axis defined by the unit vector k. By
means of quaternions, this expression can be rewritten as
!
cos(θ/2)
q= (28)
k sin(θ/2)
p0 = q ∗ p1 ∗ q̄ (29)

See for example [9], where both a convention corresponding with the column
vector and the row vector convention of homogeneous transformation matrices
are presented. In [10] the quaternions used in transformation matrices are de-
scribed and an example of a quaternion based solution to the inverse kinematics
problem for the robot arm PUMA 560 is shown.

4.4 Rigid motion

The most general move between the coordinate frames 0 and n can be described
by a pure rotation combined with a pure translation. This combination is called
a rigid motion if the rotation matrix R0n in the equation

p0 = R0n pn + dn0 (30)

is orthogonal, i.e., (R0n )T R0n = I. Important properties of the rotation matrix

R worth knowing are det(R) = 1 and the set of all 3×3 rotational matrices are
collected by in the Special Orthogonal group of order 3, called SO(3). [17]
According to [17] the rigid motion can be represented by a matrix H of the
form !
R d
H= (31)
0 1
Since R is orthogonal, the inverse transformation H −1 is defined by
!
−1 RT −RT d
H = (32)
0 1

These transformations are called homogeneous transformations. To be able to

write the transformation as a matrix multiplication, the homogeneous represen-

11
tation Pi of the vector pi is defined as
!
pi
Pi = (33)
1
(34)
Now the transformation (30) can be written as the homogeneous matrix equation
P0 = H01 P1 (35)
Combining two homogeneous transformations, e.g., p0 = R01 p1 + d10 and
p1 = R12 p2 + d21 , gives
! ! !
1 2 R01 d10 R12 d21 R01 R12 R01 d21 + d10
H0 H1 = = (36)
0 1 0 1 0 1

4.5 Differentiation of the transformation matrix

A skew symmetric matrix S is defined as
ST + S = 0 (37)
If the rotation matrix R(θ) is dependent of the variable θ, the derivative can be
described by means of the skew symmetric matrix S
dR
= SR(θ) (38)
dθ
In this report the rotation matrix R(q) depends on the joint variables q. In the
same way, a time varying matrix R(t) is described as
Ṙ(t) = S(t)R(t) (39)
with the skew symmetric rotation
matrix S(t). Since S(t) is skew symmetric, it
can also be represented as S ω(t) for the unique vector ω(t), which is the an-
gular velocity of the rotating frame with respect to the fixed frame, for example
ω0n for the tool expressed in the base frame 0. It then gives
Ṙ0n (t) = S ω0n (t) R0n (t)


(40)
which, after reordering of the terms, gives
S ω0n (t) = Ṙ0n (t)R0n (t)−1 = Ṙ0n (t)R0n (t)T


(41)



If ω(t) = ω1 (t) ω2 (t) ω3 (t) , the skew symmetric matrix S ω(t) then is defined
 
0 −ω3 (t) ω2 (t)

 
S ω(t) =   ω3 (t) 0 −ω1 (t) (42)
−ω2 (t) ω1 (t) 0
With these skew symmetric matrices it is possible to simplify many of the com-
putations when deriving the relative velocity and acceleration transformations
between coordinate frames. How to use the properties of the skew symmet-
ric matrices in order to simplify the differentiation of the Jacobian is further
described in Section 8. [17]

12
5 Denavit-Hartenberg representation
The Denavit-Hartenberg representation, also called the D-H convention, de-
scribes a systematic way to develop the forward kinematics for rigid robots.
The classical method was presented by Jacques Denavit and Richard S. Harten-
berg in 1955, see [8]. This convention to develop reference frames is commonly
used in robotic applications.
There are two slightly different approaches to the convention, the so called
standard D-H notation described in for example [17] and the modified D-H form
found in [7]. A comparison between the forms can be found in Figure 8 in [4],
where it can be seen that frame i − 1 in the standard form are denoted frame i
in the modified form. The expressions for the link transformation matrices
are different, but both notations represent a joint as two translations and two
rotations.

5.1 Homogeneous matrix

The homogeneous matrix Ai = Ai (qi ) is a function of a single joint variable qi .
It describes the transformation of the coordinates from frame i to frame i − 1
under the assumption that the joints are either revolute or prismatic. The
transformation matrix Tij that transforms the coordinates of a point expressed
in frame j to frame i can then be written as

Tij = Ai+1 Ai+2 · · · Aj−1 Aj , i<j (43)

The homogeneous transform Ai is of the form below, compare to (31)

!
i
Ri−1 dii−1
Ai = (44)
0 1

The total homogeneous transform Tij is then given by

!
j Rij dji
Ti = Ai+1 · · · Aj = , i<j (45)
0 1
where
Rij = Rii+1 · · · Rjj−1 , i<j (46)
and dj−1
i is given recursively by

dji = dij−1 + Rij−1 dj−1

j , i<j (47)

With i = 0 and j = n, (45) gives the homogeneous matrix T0n which transforms
the coordinates from frame n to frame 0
!
n R0n dn0
T0 = (48)
0 1
The position and orientation of the robot tool in the inertial coordinate frame 0
are thereby described by

T0n = A1 (q1 ) · · · An (qn ) (49)

13
which uses the current joint variable qi in every transformation step on the way
from frame n to frame 0. This results in the transformation of the vector pn
expressed in frame n to frame 0 as in the relation

pn0 = R0n pn + dn0 (50)

5.2 Standard Denavit-Hartenberg representation

Each homogeneous transformation Ai is in the D-H representation regarded as
a product of four basic transformations

Ai = Rotz,θi Transz,di Transx,ai Rotx,αi (51)

The D-H link parameters θi , ai , di and αi are defined as follows. They are all
parameters of link i and joint i.

• Angle θi : angle between xi−1 - and xi -axis measured in the plane normal
to the zi−1 -axis.

• Length ai : distance from the origin oi (frame i) to the intersection between

xi - and zi−1 -axis measured along the xi -axis.
• Offset di : distance between the origin oi−1 (frame i − 1) and the intersec-
tion of xi -axis with zi−1 -axis measured along the xi -axis.

• Twist αi : angle between zi−1 - and zi -axis measured in the plane normal
to xi . [17]

When performing the translations di , ai and the rotations θi , αi in (51) it

is important that they are applied in right order. First a rotation θi around
the z-axis is performed, then a translation di along the new z-axis followed by a
translation ai along the new x-axis and last but not least a rotation αi around
the new x-axis. See for example Figure 3-4, page 69, in [17] for an illustration
of the transformations.
Under the circumstances that the origin oi (frame 0) do not need to lie at
joint i, the coordinate frames 0, . . . , n for the robot can always be chosen in such
a way that the following two conditions are satisfied [17]

(D-H1) xi is perpendicular to zi−1

(52)
(D-H2) xi intersects zi−1

Figure 3-2, page 67, in [17] shows an illustration of the conditions.

14
 With the four basic transformations, the homogeneous transformation Ai
then can be mathematically written as
Ai = Rotz,θi Transz,di Transx,ai Rotx,αi
  
cos θi − sin θi 0 0 1 0 0 0
  
 sin θi cos θi 0 0 0 1
  0 0
= 
 0 0 1 0  0 0 1 di 
  

0 0 0 1 0 0 0 1
  
1 0 0 ai 1 0 0 0
  
 0 cos αi − sin αi 0
0 1 0 0  

0 0 1 0  0 sin αi cos αi 0
  

0 0 0 1 0 0 0 1
 
cos θi − sin θi cos αi sin θi sin αi ai cos θi
 
 sin θi cos θi cos αi − cos θi sin αi ai sin θi 
= 
 0 sin αi cos αi di 
 

0 0 0 1 (53)
which also can be collected on the form (44), repeated here for simplicity
!
i
Ri−1 dii−1
Ai =
0 1
It can directly be seen that
 
cos θi − sin θi cos αi sin θi sin αi
i
 
Ri−1 = sin θi cos θi cos αi − cos θi sin αi 
 (54)
0 sin αi cos αi
 
a cos θi
 i
dii−1 = 

 ai sin θi 
 (55)
di
The homogeneous transformation Ai = A(qi ) is a function of just one vari-
able, the generalised coordinate qi , so three of the four D-H link parameters θi ,
ai , di and αi are constant for link i and the fourth is the present joint variable.
For example the joint variable for a revolute joint is θi , whereas it is di for a
prismatic joint, see also Section 5.1 for a discussion about different types of links
and joint variables. [17]
One important thing to notice is however that choices of the coordinate
frames are not unique. [17]

6 D-H representation – a robot example

This section describes the derivation of joint variables and transformation ma-
trices for the robot IRB1400 from ABB Robotics shown in Figure 3. Information

15
regarding IRB1400 is mainly taken from [16]. In this section it is also described
how these variables and matrices are treated in the DH-toolbox implemented
in the Maple programming language.

Figure 3: The industrial robot IRB1400 with a payload of 5 kg and a maximum

range of 1.44 m. Picture from ABB Robotics public archive [1].

6.1 Link parameters and joint variables

The D-H link parameters for IRB1400 are given in Table 1 and the robot is
positioned in the configuration defined by these parameters.

Table 1: The D-H link parameters for the robot IRB1400 [16].

Joint/Link αi ai θi di
i [rad] [m] [rad] [m]
1 −π/2 0.15 0 0.475
2 0 0.6 −π/2 0
3 −π/2 0.12 0 0
4 π/2 0 0 0.72
5 −π/2 0 0 0
6 0 0 π 0.085

What are the joint variables in this example with IRB1400? They have to
be known before the velocity kinematics of the robot can be derived. Let us
say that the joint variables for this robot is, generally speaking, the variables φi
according to

T
φ = φ1 φ2 φ3 φ4 φ5 φ6 (56)
These joint variables can be determined by reasoning around the movement of
the actual robot link and then verified by a sketch of the coordinate frames for
the links in order to see that the variables gives the expected appearance and

16
structure of the robot. In this report we are satisfied with the reasoning, and
the sketch is left for prospective readers.
The movement of each link is compared to the coordinate frame for the link.
Since it is always a rotation around the z-axis in the local coordinate frame for
each link of the robot IRB1400, this gives that all joints are revolute and the
joint variable φi is equal to the joint angle θi for each joint, see Section 5.2 for
a description of the D-H representation and the variables involved.
But, are these variables the “real” D-H joint variables for the robot? Or do
we need to modify them because of the configuration of the robot IRB1400?

6.1.1 Configuration of IRB1400

Loosely speaking, there are two types of industrial robots — those with an
open kinematic chain and those who have a closed kinematic chain. Kinematic
chains is described in Section 4 and the robot is now seen as a set of rigid
links connected together by different joints, under the assumption that each
joint only has a single degree of freedom. In Section 3.2 we discussed different
types of joints and established that all types of joints can be described by only
revolute or prismatic joints with one degree of freedom each, which satisfies this
assumption.
The chain of robot links can be open, with every link connected to every
other link by one and only one path. When the kinematic chain is connected to
every other link by at least two distinct paths, it forms two or more closed-loop
chains and therefore is a closed kinematic chain. [18]
The robot IRB1400 has, as can be seen in Figure 1 or Figure 3, a mechanical
coupling between motor 3, placed in the foot of the robot, to the actual link 3.
We have a closed kinematic chain, and the structure in this particular case is
also called a parallelogram linkage structure to link 3 [17]. In practice this
parallelogram linkage structure means that joint 2 and joint 3 are coupled to
each other. If motor 2 is moved, but not motor 3, the angle between the upper
arm (x3 -axis) and zo -axis is constant, so that link 3 keeps the same orientation
with respect to the base frame, i.e., when moving motor 2 it will affect both
the variables θ2 and θ3 . The profitable idea with this parallelogram linkage
structure is that motor 3, which moves axis 3, is placed below this axis. The
upper robot links can then be made thinner and stiffer and it is easier and
cheaper to move the robot links. It also reduces the production cost since
less material is needed. A drawback with this structure is however that the
movement of the robot is more restricted and the robot cannot rotate the upper
links with an angle π (called bending backwards) — instead the whole robot
has to rotate around. [16]

6.1.2 Transformed joint variables

The D-H convention is based on the assumption that a movement of a joint does
not affect any other joint, i.e., we have an open kinematic chain. As described,
IRB1400 has a closed kinematic chain, but in this case it is of a bilinear form,
which gives the opportunity to transform its joint variables to the corresponding
D-H joint variables for the transformed, open structure.
The need of transforming the joint variables to “real” D-H variables (also
called D-H parameters in [17]) can also be seen from the fact that when all

17
variables in the D-H representation are zero, the robot is placed in a certain
position and orientation (pose). (In this case it by the way happens to be a
position that the robot never can reach.) This “zero variable pose” is however
different from the pose of the robot when its joint angles are zero, so we need
to transform the variables in order to achieve an agreement between the zero
robot pose and “zero variable pose”.

6.1.3 Mathematical summary

As a summary, in order to get the D-H parameters θi for the robot, the joint
angles φi need to be translated from its origin with the vector θ0 and trans-
formed by the matrix θT rans . Generally this gives the expression for the D-H
parameters θ [16] as follows

θ = θT rans φ + θ0 (57)

This can be performed in Maple by DHparameter, see also Section A.3. For
the IRB1400 case with mechanical coupling between joint 2 and joint 3 and the
zero pose given in Table 1, it results in
   
1 0 0 0 0 0 0
   
0 1 0 0 0 0 −π/2
   
   
0 −1 1 0 0 0  0 
θT rans = 
  and θ0 =   (58)
0 0 0 1 0 0  0 
 
   
0 0 0 0 1 0  0 
   
0 0 0 0 0 1 π
In the sequel the notation q is used for the non-constant D-H link parameters,
also called D-H joint variables, in order to, hopefully more clearly, see them as
general variables used and differentiated in for example Section 7 and Section 8
and separate them from the specific joint angles for each joint.
The D-H joint variables qi are in the IRB1400 case the joint angles θi be-
cause there are only rotations and no translations of the joints, as mentioned
in Section 6.1. And now, after the transformation of the D-H joint variables,
the robot stands in its zero pose when the joint angles of the robot are zero.
The D-H link parameters αi , ai , di and joint variables qi = θi now looks like in
Table 2.

6.1.4 Thoughts about the closed kinematic chain

In this report we have investigated certain types of closed kinematic chains that
can be described on a bilinear form. The parameter transformation in (57) is
performed at the beginning, i.e., when determining the D-H link parameters
and joint variables. Another approach may be to calculate the kinematic re-
lations using the joint variables for the actual robot and afterwards transform
the result into the “real D-H” joint variables for the corresponding, transformed
open kinematic chain.
A reasonable idea in the approach with the transformation afterwards is
that with only linear cross-connections (57) it gives just an additional matrix

18
Table 2: The D-H link parameters αi , ai , di and joint variables qi = θi for the
robot IRB1400 [16].

Joint/Link αi ai qi = θi di
i [rad] [m] [rad] [m]
1 −π/2 0.15 θ1 0.475
2 0 0.6 θ2 0
3 −π/2 0.12 θ3 0
4 π/2 0 θ4 0.72
5 −π/2 0 θ5 0
6 0 0 θ6 0.085

multiplication when calculating, e.g., the Jacobian. The offset reasonably does
not influence the Jacobian directly because the Jacobian contains derivatives,
and the offset does not contribute in these derivatives.
In Robotics Toolbox for Matlab [4] the parameter transformation has been
treated by an offset in the robot object. The offset is specified by a vector,
which is added to the joint angles within the link object. It is in other words
added before any kinematic or dynamic functions are operating on the joint
angles. The offset vector is in a similar way subtracted when needed, e.g.,
after deriving the inverse kinematics. In [4] it is also described why there is
a need for an offset; when the joint angles are zero the robot may be in an
unachievable pose. The offset then makes it possible to perform the pose of the
robot corresponding to zero joint angles.

6.1.5 How to do in Maple

Before being able to create robot links in Maple and bring them together
to a robot, we have to know something about the different types in Maple.
The input when creating a link is specified by a sequence of all allowed types
of inputs. This facilitates a lot if one know how Maple thinks about types
and operands. Therefore this section begins with a try to summarize the most
important features of types and operands in Maple, followed by a description
of how a link and a robot is created in the DH-toolbox.

Operands in Maple θ(t) and θ1 (t) are distinguished by the op command.

As is shown below the symbol θ is different parts in the two expressions. The
first operand in θ(t) is θ, but in θ1 (t), we have to investigate the first operand of
the expression, i.e., θ1 (the variable t is the second operand in the expression)
and then take the first operand of this subexpression to get θ. How to do in
Maple to pick out the symbol θ is shown below.

op(0, theta(t))
op(0, op(0, theta[1](t)))

Types in Maple It is important to know that

19
 whattype(theta(t) - 1);
whattype(theta(t) - pi);
gives the same answer, type ’+’, and one might think that they are both a sum
of a function(symbol) (θ) and numeric (−1) or symbol (−π). Instead, −π is,
seems very logical when one knows it, (−1)∗π, which is a multiplication, type ’*’
with operands of type integer (or numeric, larger type class) and symbol. − π2
is in the same way a multiplication (− 12 )∗π with operands of types fraction (or
numeric, which is a larger type class than fraction) and symbol. And while
θ(t) is of type function or the more specific type function(symbol), −θ(t)
is of type ’&*’(numeric, function(symbol)). The structured types (types
combined of defined Maple types) that are interesting as arguments for the
links are collected in Table 3 and are discussed further on when creating the
links in Maple.

Table 3: Possible arguments to the links. There are other, more general types,
but the types here are chosen because they seem to be as specific as possible
(or as general as possible, if such a property is useful).

argument type in Maple

5 numeric, (also integer)
5.0 numeric, (also float)
π symbol
π
2 ’&*’(numeric, symbol)
θ symbol
θ1 name(indexed)
θ(t) function(symbol)
θ1 (t) function(symbol)
θ(t) + 5 ’&+’(function(symbol), numeric)
θ(t) + π ’&+’(function(symbol), symbol)
π
θ(t) + 2 ’&+’(function(symbol), ’&*’(numeric, symbol))
θ(t) − 5 ’&+’(function(symbol), numeric))
θ(t) − 5.0 ’&+’(function(symbol), numeric))
θ(t) − π ’&+’(function(symbol), ’&*’(numeric, symbol))
π
θ(t) − 2 ’&+’(function(symbol), ’&*’(numeric, symbol))
θ1 − θ2 ’&+’(name(indexed), ’&*’(numeric, name(indexed)))
θ1 (t) − ’&+’(function(symbol), ’&*’(numeric, function(symbol)))
θ2 (t)
−θ(t) − 5 ’&+’(’&*’(numeric, function(symbol)), numeric)
−θ1 (t) + 0 ’&*’(numeric, function(symbol))

In Maple these two ways of writing gives the same result

20
 type(-pi, ’&*’(numeric, symbol));
type(-pi, (numeric &* symbol);

Now one example that can be a bit confusing when the reason behind it is
not known.

type(1+a, ’+’);
with(VectorCalculus);
type(1+a, ’+’);

It first shows true, but, after using the package VectorCalculus (which has a
“vector +”), the ordinary type ’+’ is changed to the plus operator in the package
VectorCalculus. So, with type(1+a, :-’+’) instead, the global, ordinary +
without any extra packages loaded, is used. One valuable comment regarding
:-’+’ is that this is an operand that invokes the operator ’+’ in the original,
global package which is not extended with other packages. Generally a special
operator in a certain package is invoked by package:-operator. A simple
example is LinearAlgebra:-Transpose(MyMatrix).
During the tests with types in Maple, we have found out the following
structure of types. The type numeric is a larger type class, containing the
subclasses integer, fraction and float.

D-H parameters First, the D-H link parameters αi , ai , di , joint variables

θi and the translation parameter vector θ0 for the robot IRB1400 as defined
in (58) are derived in Maple as follows.

DHalpha := Vector[column]([-Pi/2, 0, -Pi/2, Pi/2, -Pi/2, 0]);

DHa := Vector[column]([ 0.15, 0.6, 0.12, 0, 0, 0 ]);
DHtheta := Vector[column]([ theta[1](t), theta[2](t),
theta[3](t), theta[4](t),
theta[5](t), theta[6](t) ]);
DHd := Vector[column]([ 0.475, 0, 0, 0.72, 0, 0.085 ]);
DHtheta0 := Vector[column]([ 0, -Pi/2, 0, 0, 0, Pi ]);

The matrix θT rans in (57) must also be defined in Maple.

theta||Trans := Matrix([[1, 0, 0, 0, 0, 0],

[0, 1, 0, 0, 0, 0],
[0, -1, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]]);

Now the transformed D-H parameters θ in (57) can be calculated by means of

the function DHparameter, see Section A.3.

DHparameter(DHtheta, DHtheta0, thetaTrans);

21
The resulting D-H parameters θ in (57) are
 
θ1 (t)
 
 θ (t) − π/2 
 2 
 
−θ2 (t) + θ3 (t)
θ=  (59)
θ4 (t)
 
 
 

 θ5 (t) 

θ6 (t) + π

A robot link in Maple The structure in this package DH-toolbox is in-

spired by the structure found in Robotics Toolbox [4], where a link object is de-
 (r) or prismatic (p). Linki
fined by its D-H parameters and type of link; revolute
is therefore here formed as a list with the structure θi di ai αi jointtype
as is shown below for a robot example where the first link is prismatic and the
second link is revolute.

link1 := DHlist2link(theta[1], d[1], a[1], alpha[1], p);

link2 := DHlist2link(theta[2], d[2], a[2], alpha[2], r);

which gives the answers

h i
link1 := θ1 d1 a1 α1 p
h i
link2 := θ2 d2 a2 α2 r

of type link, defined in DH-toolbox. The link type is a list of allowed types,
see Section A.2 for the definition and Section A.4 for how to use the procedure
DHlist2link to create a link of type link from the specified link parameters.
Possible types of arguments which give type link are collected in Table 3.
Structured types in Maple is also discussed earlier in this section and can be
seen as a supplement to the table. As is clearly shown, there are lots of different
types in Maple that have to collaborate with us...
The expressions in Table 3 can be combined to one structured type, which
consists of the following types listed below in order to handle mixed orders of
the operands.

• numeric,

• symbol,

• name(indexed),

• function(symbol),

• ’&*’(numeric, {symbol, function(symbol)}),

• ’&+’({numeric, symbol, name(indexed), function(symbol),

’&*’(numeric, {symbol, name(indexed), function(symbol)})},
{numeric, symbol, name(indexed), function(symbol),
’&*’(numeric, {symbol, name(indexed), function(symbol)})}

22
 With DHvariablechange the actual D-H variable can be changed to a chosen
variable name according to following example. In Section 6.1.3 the variable name
q is used and we use it in the sequel too for convenience.

link1 := DHlist2link(theta[1], d[1], a[1], alpha[1], p);

link2 := DHlist2link(theta[2], d[2], a[2], alpha[2], r);
DHlink1 := DHvariablechange(link1, q);
DHlink2 := DHvariablechange(link2, q);

gives the answers h i

DHlink1 := θ1 q1 a1 α1 p
h i
DHlink2 := q2 d2 a2 α2 r
of type link, where the D-H joint variable θi is exchanged in the case of a
revolute link and the D-H joint variable di in the case of a prismatic link.
When the links are numerical, there is of course no exchange to the chosen D-H
variable. See also Section A.5 for the details. The creation of a link and change
of variable name can also be done directly by using DHlink, see Section A.6 and
the example below.

DHlink1 := DHlink(theta[1], d[1], a[1], alpha[1], p, q);

h i
gives as above DHlink1 := θ1 q1 a1 α1 p of type link.
A more complicated example is when the transformed D-H parameters (57)
are used.

DHlink(-theta[2](t) + theta[3](t), d[3], a[3], alpha[3], r, q);

DHlink(-theta[2](t) + theta[3](t), d[3], a[3], alpha[3], p, q);
h i h i
It gives −q2 (t) + q3 (t) d3 a3 α3 r and −θ2 (t) + θ3 (t) q3 a3 α3 p .

A robot in Maple A robot model is constructed by placing a number of

robot links with proper characteristics (i.e., of type link) after each other in
a list. The procedure DHrobot takes at least one link as argument, and can
also take arbitrarily many links. For example, with link1 and link2 defined as
above a robot can be defined as

robot := DHrobot(link1, link2);

hh i h ii
which gives the output robot := θ1 q1 a1 α1 p , q2 d2 a2 α2 r
of type robot, see the definition in Section A.2. DHrobot can be seen in Sec-
tion A.7.

6.2 Transformation matrices

First, as an example, the homogeneous transformations for the first two links in
the IRB1400 example according to (53) are examined when the joint variables
φi (57) are zero, i.e., the zero pose of the robot and D-H joint variables qi = θi

23
are constant.
 
1 0 0 0.15
 
0 0 1 0 
A1 ≈   (60)
0 −1 0 0.475
 

0 0 0 1
 
0 1 0 0
 
−1 0 0 −0.6
A2 ≈   (61)
0 0 1 0 
 

0 0 0 1
These matrices describe the two coordinate frames associated to the two links
and the way to go between them. T02 then gives the position d20 and orientation
R02 of the tool frame in coordinate frame 2 expressed in the base coordinates
(inertial coordinate frame 0) as shown in (45) and below for this example
 
0 1 0 0.150
 
0 0 1 0 
T02 = A1 A2 ≈   (62)
1 0 0 1.075
 

0 0 0 1
 
0 1 0
 
0 0 1
R02 = 

 (63)
1 0 0
 

0 0 0
 
0.150
 
0 
d20 = 

 (64)
1.075
 

T03 gives the same for frame 3 expressed in the base coordinates (frame 0)
 
0 0 1 0.150
 
0 −1 0 0 
T03 = A1 A2 A3 ≈ 

 (65)
1 0 0 1.195
 

0 0 0 1

The transformation matrix for the robot tool (frame 6) relative to the base
frame 0 is  
0 0 1 0.955
 
0 1 0 0 
T06 = A1 A2 A3 A4 A5 A6 ≈   (66)
−1 0 0 1.195
 

0 0 0 1

24
From this it can be seen that the last column represents the position of the
robot tool relative to the base frame, i.e., d60 as described in (45)
 
0.955
d60 ≈ 
 
 0 
 (67)
1.195
which means that the position of the robot tool relative to the base frame of the
robot is 0.995 m in the x0 -direction, 0 m in the y0 -direction and 1.195 m in the
z0 -direction. Compared to the blueprint of the actual robot IRB1400 in [16],
this gives the same result.

6.2.1 How to do in Maple

Homogeneous matrices The homogeneous matrices are described in Sec-
tion 3.2 and are in the DH-toolbox calcluated by means of the procedure
DHmatrixA, see Section A.8. For example, a revolute link with the D-H joint
variable θ1 exchanged to the variable name q1
link1 := DHlink(theta[1], d[1], a[1], alpha[1], r, q);
A[1] := DHmatrixA(link1);
gives the homogeneous matrix (53)
 
cos q1 − sin q1 cos α1 sin q1 sin α1 a1 cos q1
 
 sin q1 cos q1 cos α1 − cos q1 sin α1 a1 sin q1 
A1 =   (68)
 0 sin α1 cos α1 d1 
 

0 0 0 1
With all D-H parameters constant according to Table 1, the first homogeneous
matrix A1 is given by
link1 := [-Pi/2, 0.15, 0, 0.475, r];
A[1] := DHmatrixA(link1);
which gives the same result as in (60). If, as an illustrative example of an
interesting feature in Maple, all link parameters are of type integer
link1 := [1, 2, 3, 4, r];
A[1] := DHmatrixA(link1);
the result becomes
 
cos(1) − sin(1) cos(4) sin(1) sin(4) 3 cos(1)
 
 sin(1) cos(1) cos(4) − cos(1) sin(4) 3 sin(1) 
A1 = 
  (69)
 0 sin(4) cos(4) 2 


0 0 0 1
because Maple prefers to have the computations in function/integer form in-
stead of compute the answer numerically and have to use floating-point numbers.
If a numerical value is desired, just print evalf(A[1]) to evaluate numerically
using floating-point arithmetic.

25
Transformation matrices Now the general transformation matrices T0i (45)
are derived in Maple. The transformation matrices below are given with
DHmatrixT, see Section A.9. In this example the robot consists of the first
two links and is created with the commands
link1 := [theta[1], 0.475, 0.15, -Pi/2, r];
link2 := [theta[2] - Pi/2, 0, 0.6, 0, r];
myrobot := DHrobot(link1, link2);
T := DHmatrixT(myrobot);
and the result is of type robot
hh i h ii
myrobot := θ1 (t) 0.475 0.15 − π2 r , θ2 (t) − π
0 0.6 0 r
2

and the transformation matrix T, which is a table of the transformation matrices

T1 and T2 . The respective matrices can be viewed by typing the first and second
element in the table, i.e., T[1] and T[2], which gives
 
cos θ1 0 − sin θ1 0.15 cos θ1
 
1
 sin θ1 0 cos θ1 0.15 sin θ1 
T 0 = A1 =    (70)
 0 −1 0 0.475 


0 0 0 1

T02 = A1 A2
  
cos θ1 0 − sin θ1 0.15 cos θ1 sin θ2 cos θ2 0 0.6 sin θ2
  
 − cos θ2 −0.6 cos θ2 
 sin θ1 0 cos θ1 0.15 sin θ1   sin θ2 0
= 
 0 −1 0 0.475   0 0 1 0
  

0 0 0 1 0 0 0 1

 
cos θ1 sin θ2 cos θ1 cos θ2 − sin θ1 0.6 cos θ1 sin θ2 + 0.15 cos θ1
 
 sin θ1 sin θ2 sin θ1 cos θ2 cos θ1 0.6 sin θ1 sin θ2 + 0.15 sin θ1 
= 
 cos θ2 − sin θ2 0 0.475 + 0.6 cos θ2
 

0 0 0 1
(71)
As can be seen, the size of the expressions are increasing, so the transforma-
tion matrices for the robot with more than just the two first links will not be
presented here (too large!).

6.3 Rigid motion

In the IRB1400 example, we have as previously, the D-H parameters given
according to (57) with the parameter values in Table 2. As an example of how
the transformation matrices derived above can be used, we study the vector
 
1
 
p2 = 1

 (72)
1

26
expressed in coordinate frame 2. It is transformed to the base frame 0 by means
of the transformation matrix
 
cos θ1 sin θ2 cos θ1 cos θ2 − sin θ1
R02 = 
 
 sin θ1 sin θ2 sin θ1 cos θ2 cos θ1  (73)
cos θ2 − sin θ2 0
and the vector from the origin to coordinate frame 2
 
0.6 cos θ1 sin θ2 + 0.15 cos θ1
d20 = 
 
 0.6 sin θ1 sin θ2 + 0.15 sin θ1 
 (74)
0.475 + 0.6 cos θ2
Using (50) the vector expressed in frame 0 can now be calculated as
p20 = R02 p2 + d20 (75)
This gives
 
1.6 cos θ1 sin θ2 + cos θ1 cos θ2 − sin θ1 + 0.15 cos θ1
p20 = 
 
 1.6 sin θ1 sin θ2 + sin θ1 cos θ2 + cos θ1 + 0.15 sin θ1 
 (76)
1.6 cos θ2 − sin θ2 + 0.475

6.3.1 How to do in Maple

In Maple this can be calculated by the procedures DHpositionT, DHrotationT
and DHtransformationT, see Section A.10, Section A.11 and Section A.12 and
below how to use them. This example uses the first two links with the robot in
the zero pose, as in Section 6.3.
link1 := [theta[1], 0.475, 0.15, -Pi/2, r];
link2 := [theta[2] - Pi/2, 0, 0.6, 0, r];
myrobot := DHrobot(link1, link2);
T := DHmatrixT(myrobot);

p[2] := Vector[column]([1,1,1]);

R02 := DHrotationT(T[2]);
d02 := DHpositionT(T[2]);
p[0] := DHtransformationT(p[2], T[2]);
which gives the same result as in (76).

7 Velocity kinematics
The velocity kinematics relates the linear and angular velocities of a point on
the robot to the joint velocities. The D-H joint variables represent the relative
displacement between adjoining links. In this section the robot has n links. The
industrial robot IRB1400 is used to illustrate the theory, and the joint variables
are qi = θi in this case. If nothing else is mentioned, the reference used in this
section is [17].

27
7.1 Manipulator Jacobian
The forward kinematic equations derived in Section 5 determine the position
and orientation of the robot tool given the D-H joint variables. The Jacobian of
this function determines the linear and angular velocities of a point on the robot
related to the joint velocities and it is one of the most important quantities in the
analysis and control of the robot motion. It is used in many aspects in robot
control, like planning and execution of smooth trajectories, determination of
singular configurations, execution of coordinated motion, derivation of dynamic
equations of motion and last but not least to transform the forces and torques
from the tool to the joints. [17]
Generally the n-link industrial robot has the joint variables q = (q1 . . . qn )T .
The transformation matrix from the robot tool frame n to the base frame 0
in (45) is rewritten below for simplicity with D-H joint variables qi
!
n R0n (q) dn0 (q)
T0 (q) =
0 1
where
dn0 = R0i−1 dni−1 + di−1
0 (77)
When the coordinates already are expressed in the base frame, no transforma-
tion to the base frame is needed and we therefore have T00 (q) = I. This gives
according to the relation above that R00 (q) = I and d00 (q) = (0 0 0)T .
The angular velocity vector ω0n is defined as the skew symmetric matrix (see
also Section 4.5)
S(ω0n ) = Ṙ0n (R0n )T (78)
and the linear velocity of the robot tool is
v0n = d˙n0 (79)
ddn
where d˙n0 means the time derivative dt0 . Written on the form
v0n = Jv q̇ (80)
ω0n = Jω q̇ (81)
(80) and (81) can be combined to
! !
v0n Jv
= q̇ = J0n q̇ (82)
ω0n Jω
J0n is called the Manipulator Jacobian, or just the Jacobian. It is a 6 × n-matrix
because it represents the instantaneous transformation between the n-vector of
joint velocities and the 6-vector which describes the linear and angular velocities
of the robot tool. In the following sections it is described how the matrices Jv
for the linear velocities and Jω for the angular velocities can be derived.

7.2 Linear velocity

The linear velocity of the robot tool is described by just d˙n0 , see (79). Using the
chain rule of differentiation the result is
n
X ∂dn0
d˙n0 = q˙i (83)
i=1
∂qi

28
The differentiation (83) combined with (80)gives that the ith column of Jv is
∂dn
∂qi . This expression is the linear velocity if q˙i is one and all the other q˙j are
0

zero, which is achieved if the ith joint is actuated at unit velocity and all the
other joints are fixed. There are two cases; a revolute joint and a prismatic joint
and the respective joint is described by the relation (77).

Prismatic joint Study (77), repeated here

dn0 = R0i−1 dni−1 + di−1

0

The prismatic joint i gives R0j−1 independent of the joint variable qi = di for
all j. This can be seen when studying the homogeneous matrices Ai , see (53),
which is also repeated here for simplicity.
 
cos θi − sin θi cos αi sin θi sin αi ai cos θi
 
 sin θi cos θi cos αi − cos θi sin αi ai sin θi 
Ai = 
 
 0 sin αi cos αi di 


0 0 0 1

The D-H joint variable di is only in the third row and fourth column, A3,4 , and
there are zeros in element A3,1 , A4,1 , A4,2 and A4,3 . When two matrices of this
structure are multiplied, the di -variables are all in the fourth column, which
does not affect the behaviour of the R matrices, which is just the upper left
3 × 3-matrix.
Between the coordinate frames i and i − 1 we have the relation

dii−1 = Ri−1
i
ai x + di z (84)

which is a transformation from coordinate frame i to frame i − 1, which can be

i
described on the form (30), i.e., pi−1 = Ri−1 pi + dii−1 . In this case (84) the
i
translated and rotated vector results in di−1 , with the translation term di z (di is
the D-H variable called offset) which is the offset from the origin in expressed
cordinate frame i − 1 and ai x (ai is the D-H variable called lenght) which is
the term to be rotated. The unit vectors x = (1 0 0)T and z = (0 0 1)T are
expressed in frame i − 1.
If all other joints but the ith one are fixed, we have the time derivatives
of di equal to zero for all joints except the ith one, i.e., d˙j = 0, j 6= i. The
differentiation of (77) then together with (84) gives

d˙n0 = R0i−1 d˙ii−1 = d˙i R0i−1 z = d˙i zi−1 (85)

where di is the D-H variable called offset and zi−1 is the resulting vector in
the base frame when the unit vector z = (0 0 1)T in coordinate frame i − 1 is
expressed in the base frame with the transformation matrix R0i−1 . Compared
to (83), with qi = di for the prismatic joint, this gives
 
0
∂dn0 i−1 i−1  
 
= zi−1 = R0 z = R0 0 (86)
∂qi
1

29
Revolute joint The revolute joint i is described by the relation (77), repeated
here for convenience
dn0 = R0i−1 dni−1 + d0i−1
If only the ith joint is actuated, all joint angles except θi are fixed (constant)
and di−1
0 and R0i−1 are constant [16]. Differentiation of (77) now gives
d˙n0 = R0i−1 d˙ni−1 (87)
Since the joint is revolute and therefore the motion of link i is a rotation qi = θi
about the axis zi−1 , according to [16] it gives
d˙ni−1 = θ̇i z × dni−1 (88)
with z = (0 0 1)T in coordinate frame i − 1 as usual. When combining these
equations the result, with R0i−1 z = zi−1 , is
d˙n0 = R0i−1 θ̇i z × dni−1 = θ̇i R0i−1 z × R0i−1 dni−1 = θ̇i zi−1 × (dn0 − di−1


0 ) (89)
The last equality can be explained by the verbal picture that the positon of
the tool, expressed in the base frame is dno . This is also the position of the link
position next to the tool expressed in the base frame, plus the position from this
second last link to the last tool position, i.e., dn0 = di−1
0 + dni−1 . Thsi relation
can also be seen in for example Figure 5-1 [17].
From (83), with qi = θi , it can be seen that
∂dn0
= zi−1 × (dn0 − di−1
0 ) (90)
∂qi

Expression for linear velocity Comparison with (80), the part of the Ja-
cobian for the linear velocity, Jv , can now be given as
 
Jv = Jv1 . . . Jvn (91)

where Jvi is

∂dn0 z ,
i−1 prismatic joint
Jvi = = (92)
∂qi i−1
zi−1 × (dn0 − d0 ), revolute joint


z0 is of course the unit vector z = (0 0 1)T in the base frame and n is the
number of links in the robot, as mentioned before.

7.2.1 How to do in Maple

This section is a relatively independent continuation of the examples in Maple
described in former sections. In the IRB1400 case all n = 6 joints are revolute,
as mentioned in Section 6.1. For example calculating the second column Jv2
according to (92) gives the relation
Jv2 = z1 × (d60 − d10 ) (93)
where z1 = R01 with the unit vector z = (0 0 1)T expressed in frame i − 1. (Here
z is named zunit just in order to avoid problems in Maple with different sizes
and indices of the z-variable in the workspace.) In Maple this calculation looks
like

30
 zunit := Vector[column]([0, 0, 1]);

T := DHmatrixT(robot);
R01 := DHmatrixT(T[1]);
z[1] := [Link];

d01 := DHpositionT(T[1]);
d06 := DHpositionT(T[6]);

Jv2 := CrossProduct(z[1], (d06-d01));

The other elements in the linear part of the Jacobian are caluclated in the same
way. The symbolic result is however several (5-6) pages of Maple output,
because the transformation matrices T0i (and thereby di0 ) grow with increasing
index i. It is desirable to avoid deriving these expressions by hand, and instead
use a computer algebra tool, e.g., Maple.
In order to be able to show something from an overall picture, the D-H
link parameters for the robot in zero pose, given in Table 1, is used. Using
the procedure DHlinearjacobian, see Section A.13, in order to calculated the
linear part of the Jacobian gives

link1 := DHlink(0, 0.475, 0.15, -Pi/2, r);

link2 := DHlink(-Pi/2, 0, 0.6, 0, r );
link3 := DHlink(0, 0, 0.12, -Pi/2, r);
link4 := DHlink(0, 0.72, 0, Pi/2, r);
link5 := DHlink(0, 0, 0, -Pi/2, r);
link6 := DHlink(Pi, 0.085, 0, 0, r);

myrobot := DHrobot(link1, link2, link3, link4, link5, link6);

Jv := DHlinearjacobian(myrobot);

This results in the linear part of the Jacobian as follows

 
0 0.7200 0.1200 0 0 0
 
Jv ≈ 0.9550
 0 0 0 0 0 (94)
0 −0.8050 −0.8050 0 −0.0850 0

which corresponds to the result in [16].

7.3 Angular velocity

Angular velocities can be added vectorially if they all are expressed in a common
coordinate frame [17]. It is then possible to express the angular velocity of each
link in the base frame and then sum them to the total angular velocity of the
robot tool.

Prismatic joint If for example the ith joint is prismatic, there is a pure
translation and therefore the angular velocity vector is zero according to
i
ωi−1 = 0, prismatic joint (95)

31
Revolute joint If the ith joint instead is revolute, the joint variable is qi = θi
and the z-axis is the axis of rotation. The angular velocity expressed in the
frame i − 1 is
i
ωi−1 = q̇i z, revolute joint (96)
where z is the unit vector (0 0 1)T in coordinate frame i − 1.

Expression for angular velocity Summing all the angular velocities from
each link gives the following angular velocity of the robot tool for the robot with
n links [17]
n
X n
X
ω0n = ρ1 θ̇1 z + ρ2 θ̇2 R01 z + . . . + ρn θ̇n R0n−1 z = ρi θ̇i zi−1 = ρi q̇i zi−1 (97)
i−1 i−1

where 
1, revolute joint
ρi = (98)
0, prismatic joint


The following equation describes how the unit vector z in coordinate frame i − 1
is expressed in the base frame by means of the transformation matrix R0i−1 as
in the case of the linear velocity. The resulting vector in the base frame is then
called zi−1 according to
zi−1 = R0i−1 z (99)
Comparison between (81) and (97) now gives the lower angular half Jω of the
Jacobian as
 
Jω = ρ1 z0 . . . ρn zn−1 , (100)

0, prismatic joint,
ρi = (101)
1, revolute joint.


which is a matrix of the unit vectors z for each coordinate frame transformed
into the base frame. z0 is also here the unit vector z in the base frame.

7.3.1 How to do in Maple

The robot IRB1400 has only revolute joints, as described in Section 6.1. When
we take more than two links into account in the industrial robot model, the
expressions for the Jacobian become very large, because they are functions of
the D-H joint variables qi and the large transformation matrices (43). There-
fore, just in order to see how the technique works, the first four links of the
robot IRB1400 are examined in Maple. The D-H variables are here manually
transformed to “real” D-H variables, this can of course also be done with the
procedure DHparameter, as described in Section 6. This gives the first four links
and the corresponding robot model as
link1 := DHlink(q[1](t), 0.475, 0.15, -Pi/2, r);
link2 := DHlink(q[2](t)-Pi/2, 0, 0.6, 0, r );
link3 := DHlink(-q[2](t)+q[3](t), 0, 0.12, -Pi/2, r);
link4 := DHlink(q[4](t), 0.72, 0, Pi/2, r);
myrobot := DHrobot(link1, link2, link3, link4);

32
The unit vector z in each coordinate system and the transformation

z1 = R01 z (102)

is in Maple computed as below. Here we use the name zunit instead of z in

order to avoid problems with the variable names in the Maple workspace.
T := DHmatrixT(myrobot);
zunit := Vector[column]([0, 0, 1]);
R[1] := DHrotationT(T[1]);
z[1] := R[1].zunit;
The part of the angular Jacobian for the second joint, Jω2 , then becomes
 
− sin q1 (t)
 
Jω2 =  cos q1 (t) 
 (103)
0

With DHangularjacobian, see Section A.14, the part Jω of the Jacobian for the
four first joints can easily be calculated as
Jomega := DHangularjacobian(myrobot);
with the result
 
0 − sin q1 (t) − sin q1 (t) Jω14
 
Jω = 
0 cos q1 (t) cos q1 (t) Jω24 
 (104)
1 0 0 Jω34

where



Jω14 = cos q1 (t) sin q2 (t) sin q2 (t) − q3 (t) + cos q1 (t) cos q2 (t) cos q2 (t) − q3 (t)



Jω24 = sin q1 (t) sin q2 (t) sin q2 (t) − q3 (t) + sin q1 (t) cos q2 (t) cos q2 (t) − q3 (t)



Jω34 = cos q2 (t) sin q2 (t) − q3 (t) − sin q2 (t) cos q2 (t) − q3 (t)

A numerical example, with the numerical D-H link parameters for the robot
in zero pose, given in Table 1 as in the example with the linear part of the
Jacobian, gives  
0 0 0 1 0 1
 
Jω = 0
 1 1 0 1 0
 (105)
1 0 0 0 0 0
which also corresponds to the result in [16].

7.4 Resulting Jacobian

In this section the examples with the linear and angular Jacobian in Maple
are collected and the procedures DHlinearjacobian and DHangularjacobian
are combined to DHjacobian, which calculates the whole Jacobian directly. See
also Section A.15 for a description in Maple code.

33
7.4.1 How to do in Maple
Since the resulting symbolic Jacobian for a robot with 6 links is very large
(about 9 pages of Maple output code), a numerical example is somewhat more
illustrative here. First the links are defined, the robot model is created and then
the Jacobian can be derived using DHjacobian.
link1 := DHlink(0, 0.475, 0.15, -Pi/2, r);
link2 := DHlink(-Pi/2, 0, 0.6, 0, r );
link3 := DHlink(0, 0, 0.12, -Pi/2, r);
link4 := DHlink(0, 0.72, 0, Pi/2, r);
link5 := DHlink(0, 0, 0, -Pi/2, r);
link6 := DHlink(Pi, 0.085, 0, 0, r);

myrobot := DHrobot(link1, link2, link3, link4, link5, link6);

J := DHjacobian(myrobot);
The resulting Jacobian becomes
 
0 0.7200 0.1200 0 0 0
 
0.9550 0 0 0 0 0
!  


Jv 0 −0.8050 −0.8050 0 −0.0850 0
J06 =

=  (106)
Jω 0 0 0 1 0 1
 

 
 0 1 1 0 1 0
 
1 0 0 0 0 0
This can be compared to the explicit linear part (94) and angular part (105) of
the Jacobian, which of course are the same as the linear and angular part of the
matrix above.
If we for a moment pretend that the robot consists of only prismatic joints,
we only change r to p in the link type definitions above in the Maple code.
The corresponding Jacobian then becomes
 
0 0 0 1 0 1
 
0 1 1 0 1 0 
 
 
6
 1 0 0 0 0 0 
J0 =   (107)
0 0 0 0 0 0 

 
0 0 0 0 0 0 
 
0 0 0 0 0 0
As can be seen, the Jacobian now has a simpler structure, because the angular
part now only consists of zeros. The linear part also avoids the more complicated
cross product zi−1 × (dn0 − di−1
0 ), where n is the number of links of the robot.

7.5 Discussion about the Jacobian

Since the Jacobian is an important quantity in robot modelling, analysis and
control, see also Section 7.1, there are of course many interesting things to know
about it. In this section some of the ideas and thoughts about the Jacobian are
discussed. The main source in this section is [17] if nothing else is mentioned.

34
7.5.1 Interpretation of the Jacobian
What kind of information can be obtained from the Jacobian? From (82),
! !
v0n Jv
Ẋ = = q̇ = J0n q̇
ω0n Jω

it can be seen that J0n describes how the linear and angular velocities are related
to the derivatives of the joint variables qi . For example the first element in the
upper left corner of J0n describes how the first joint is related to the derivative
of the first joint variable q1 .
The relation (82) can also be seen as a transformation between the joint
velocities and the linear and angular velocities of the robot tool [17] (the robot
tool velocities Ẋ). Infinitesimally it defines a linear transformation

dX = J(q)dq

between the differentials dq and dX. The first element of v0n is for example the
linear velocity in x-direction in the base frame, vx .
Sometimes velocities for other positions than the tool position are sought.
The calculations resulting in the Jacobian are as usual, the difference is to
involve only the links up to that point, i.e., n = ndesired . It is also shown in
the examples with the angular part of the Jacobian, Jω , where it is calculated
for the first four joints. Then the linear and angular velocity for that position
of course only depends on the speed of joint numer 1, 2, 3 and 4.
In [11] there is an example of how the kinematics for the Stanford manipu-
lator can be derived in another way in Maple. This is shortly summarised in
Section B.1 for the interested reader.

7.5.2 Singularities
The forward kinematic equations are systematically derived by the D-H conven-
tion, as described in this report. The resulting equations are dependent of the
chosen coordinate frames, but the robot configurations themselves are geomet-
ric quantities that are independent of the chosen frames. When working with
robots, it is unavoidable to end up in singularities, also called singular configu-
rations. At a singular configuration, a vector F in the nullspace of J T does not
produce any torque and therefore also no rotation about the joints. There are
also directions in the cartesian space where the robot cannot exert forces. This
happens when columns in the Jacobian are linearly dependent and the actual
configuration q gives a decreasing rank of J. For example a robot with 6 links
gives the Jacobian (82) as a 6 × 6-matrix with configuration q. It is singular if
and only if
det J(q) = 0 (108)
Identifying the singularities is important in robot control, since at singular
configurations

• certain directions of motions may be unreachable,

• bounded gripper velocities may correspond to unbounded joint velocities,

35
 • bounded gripper forces and torques may correspond to unbounded joint
torques,

• the robot often is on the boundary of its workspace,

• points in the robot workspace may be unreachable under small perturba-

tions of the link parameters.

Also near singularities a unique solution to the inverse kinematics problem does,
i.e., to calculate the joint configuration of the robot from the position and
orientation of the robot tool, does not exist. Near singularities there may be
no solution or infinitely many solutions to the problem [17]. This is a typical
behaviour in a singularity; we have a configuration where a rotation in a certain
joint can be fully counteracted by rotating another joint in “opposite direction”.
When dealing with robots with spherical wrists, this makes it possible to split
the determination of the singular configurations into two simpler problems; arm
singularities and wrist singularities. Arm singularities are singularities from
the motion of the arm (the first three or more links) and wrist singularities
results from motion of the spherical wrist. As an example of how to split the
configuration, we study a robot with 6 links. The robot then consists of an arm
with three degrees of freedom and a spherical wrist with the remaining three
degrees of freedom.
The wrist singularity only appears when two revolute joints are collinear
anywhere, since an equal and opposite rotation about the axes gives no net
motion of the robot tool. This singularity is unavoidable without having me-
chanical limits that prevents the axes from being collinear. Arm singularities
occur for example when the wrist center intersects the axis of the base rota-
tion, z0 . All arm singularities can of course be calculated from the part of the
Jacobian relating to the wrist, see [17] for a further discussion.

8 Acceleration equations
Deriving the Jacobian
! !
ẋn0 v0n
Ẋ = = = J0n q̇ (109)
ω0n ω0n

gives the linear and angular acceleration [17] according to

!
ẍn0 d n d n

J0 (q)q̇ = J0n (q)q̈ +


Ẍ = = J0 (q) q̇ (110)
ω̇0n dt dt
X n 
∂
= J0n (q)q̈ + J0n (q) q˙i q̇


(111)
i=1
∂qi

The inverse accelerations q̈ can now be determined under the assumption that
we have a non-singular Jacobian J(q). Then the following relation holds [16]
 
n

−1 d n

q̈ = J0 (q) Ẍ − J (q) q̇ (112)
dt 0

36
 Using an accelerometer at the robot tool gives the measurement relation
!
θ
yt = h(xt ) + et = + et , (113)
χ̈

The vector yt contains all measured signals, and some measurement noise et
is added. θ is a vector containing the measured joint angles and χ̈ describes
the acceleration vector in the local coordinate frame of the accelerometer. A
comparison to equation (110) shows a need of computing the time derivatives
of the Jacobian and considering the complexity of the Jacobian it is clear that
this derivation is impossible to do by hand. This expression is also used when
computing the Extended Kalman filter (EKF). See more details in [12].
The calculations of the acceleration equations are left for the following report,
and therefore we leave them for the moment.

9 Summary of example IRB1400 in Maple

In this section it is shown how to do the calculations step by step by means of the
developed procedures in DH-toolbox in Maple, as an overwiev and summary
of the steps in the symbolic calculations. It is then compared to how these steps
can be calculated as concentrated as possible by the procedures.

9.1 Step by step

The robot has n links and the D-H parameters angle θi , length ai , offset di and
twist αi are assumed to be known for each link of the robot. The numerical
link parameters are from Table 1. In order to avoid huge expressions, and show
the calculation steps a bit clearer, the robot in this example is assumed to have
n = 2 revolute links. The robot does not have a parallelogram linkage structure
with a closed kinematic chain, which means that the transformation matrix
θT rans in (57)
θ = θT rans φ + θ0
is !
1 0
θT rans =
0 1
However, the parameters have to be translated so the “real” D-H parameters
are null when the robot stands in its zero pose (like an L, flipped upside down).
This means that θ0 is !
0
θ0 =
− π2
First, we define the non-translated link parameters φ (57)

phi[1];
d[1] := 0.475;
a[1] := 0.15;
alpha[1] := -Pi/2;

37
 phi[2];
d[2] := 0;
a[2] := 0.6;
alpha[2] := 0;

and the translation and transformation matrices

thetaTrans := Matrix([1, 0], [0, 1]);

theta0 := Vector[column](0, -Pi/2);

and we translate the link parameters φ to the D-H parameters θ according to

DHtheta := DHparameter(phi, theta0, thetaTrans);

Next the links can be determined as follows. The last letter describes the type
of the link – prismatic (p) or revolute (r).

link1 := DHlists2link(DHtheta[1], d[1], a[1], alpha[1], r);

link2 := DHlist2link(DHtheta[2], d[2], a[2], alpha[2], r);

which gives a list of type link, consisting of the D-H joint variables, link pa-
rameters and link type parameter. The actual D-H variable, in this case θi , is
replaced by a chosen variable name q

DHlink1 := DHvariablechange(link1, q);

DHlink2 := DHvariablechange(link2, q);

The answer i still of type link, just the variable names θi are exchanged to qi .
These two steps DHlist2link and DHvariablechange can also be performed
by only one procedure, DHlink as follows

DHlink1 := DHvariablechange(DHtheta[1], d[1], a[1], alpha[1], r, q);

DHlink2 := DHvariablechange(DHtheta[2], d[2], a[2], alpha[2], r, q);

The homogeneous transformation matrices Ai can now be determined for each

link as follows

A[1] := DHmatrixA(DHlink1);
A[2] := DHmatrixA(DHlink2);

Next step is to create the robot by collecting the links in a list

robot := DHrobot(DHlink1, DHlink2);

The transformation matrices Ti are then calculated as

T := DHmatrixT(robot);

The respective T -matrices for the links are collected in a table, and if for example
T2 is needed, just type T2 := T[2]. If we in the following step are interested in
calculating the relation p20 = R02 p2 +d20 with for example the vector p2 = (1 1 1)T ,
the answer p20 (called p[0]) is given by

p[2] := Vector[column]([1,1,1]);
p[0] := DHtransformationT(p[2], T[2]);

38
If instead the transformation matrix R02 or the translation vector d20 are the
sought quantities
R02 := DHrotationT(T[2]);
d02 := DHpositionT(T[2]);
Last but not least how to calculate the different parts of the Jacobian (82)
! !
v0n Jv
= q̇ = J0n q̇
n
ω0 Jω

The linear part, Jv , and angular part, Jω are calculated as follows

Jv := DHlinearjacobian(robot);
Jw := DHangularjacobian(robot);
The resulting Jacobian can also easily be determined by
J := DHjacobian(robot);

10 Concluding remarks and future work

The work described in this report is a first step towards a toolbox, called
DH-toolbox, for industrial robot modelling in the computer algebra tool Maple,
used for deriving different representations of robot structures with mechanical
flexibilities. In the toolbox it shall be possible to develop nonlinear as well as
linearized state space models and also choose different locations for sensors. For
example the models shall give a structure that is suitable for example for the
System Identification Toolbox in Matlab [14].
A realistic ambition is for example to first do all steps for a robot model with
two axis, first without joint flexibilities and then incorporate them. Thereafter
the same is performed for a robot model with three axis without/with joint
flexibilities.
Future work includes to further develop the Maple functions and make
a package with a user friendly interface. It also means to develop an export
function where the result can be exported in a format that can be read from
Matlab and/or C for including the results, e.g., in an EKF algorithm for state
estimation.

References
[1] ABB. Open public archive of robot images. [Link], October 2006.
[2] Daniel Ankelhed and Lars Stenlind. Tillståndsskattning i robotmodell med
accelerometrar. Master’s thesis no LiTH-ISY-EX-05/3650, Department of
Electrical Engineering, Linköpings universitet, Sweden, 2005.
[3] Adam Bienkowski and Krzysztof Kozlowski. A toolbox for teaching and
researching robotics. In Proceedings of Worldwide Mathematica Conference,
Chicago, USA, 1998.

39
 [4] Peter I. Corke. A robotics toolbox for MATLAB. IEEE Robotics and Au-
tomation Magazine, 3(1):24–32, March 1996. Main reference for Robotics
Toolbox!

[5] Peter I. Corke. Symbolic algebra for manipulator dynamics. Technical

report, CSIRO Division of Manufacturing Technology, Australia, August
1996.

[6] Peter I. Corke. A symbolic and numeric procedure for manipulator rigid-
body dynamic significance analysis and simplification. Robotica, 16:589–
594, 1998.

[7] John J. Craig. Introduction to robotics mechanics and control. Addison-

WesleyPublishing Company, Inc, 2 edition, 1989.

[8] Jacques Denavit and Richard S. Hartenberg. A kinematic notation for

lower-pair mechanisms based on matrices. ASME Journal of Applied Me-
chanics, pages 215–221, June 1955.

[9] Karol Dobrovodsky. Quaternion position representation in robot kinematic

structures. In Proceedings of IEEE International Conference on Control,
volume 1, pages 561–564, Coventry, UK, March 1994.

[10] Janez Funda, Russel H. Taylor, and Richard P. Paul. On homogeneous

transforms, quaternions, and computational efficiency. IEEE Transactions
on Robotics and Automation, 6(3):382–388, June 1990.

[11] André Heck. Introduction to Maple. Springer Verlag, New York, 1993.

[12] Rickard Karlsson and Mikael Norrlöf. Bayesian state estimation of a flexible
industrial robot. Technical Report LiTH-ISY-R-2677, Department of Elec-
trical Engineering, Linköpings universitet, Sweden, SE-581 83 Linköping,
Sweden, January 2005.

[13] Maple, 2007. [Link]

[14] The MathWorks. Matlab The Language of Technical Computing. Version

7, 2007.

[15] John F. Nethery and Mark W. Spong. Robotica: A Mathematica package

for robot analysis. IEEE Robotics and Automation Magazine, 1(1):13–20,
1994.

[16] Mikael Norrlöf. Modeling of industrial robots. Technical Report LiTH-

ISY-R-2208, Department of Electrical Engineering, Linköpings universitet,
Sweden, December 1999.

[17] Mark W. Spong and M. Vidyasagar. Robot Dynamics and Control. John
Wiley & Sons, Ltd, New York, 1989.

[18] Lung-Wen Tsai. ROBOT ANALYSIS The Mechanics of Serial and Parallel
Manipulators. John Wiley & Sons, Ltd, New York, 1999.

40
A Malpe implementation
A.1 [Link]
# DHpackage as a module, works in Maple XI.
# Defines a package (based on a module) named DH which contains
# different procedures dealing with the computation of Denavit-
# Hartenberg parameters and transformation matrices.
#
# Every procedure is called with the name, for example
# DH:-DHTransformationT or DH[DHTransformationT]. When loaded
# to the Workspace, the exported procedures can be called whithout
# packagename DH.

DH := module()
description "Module (DH) which contains different procedures
dealing with the computation of Denavit-Hartenberg
parameters and transformation matrices.";

# exports all procedures

export
DHparameter,
DHlist2link,
DHvariablechange,
DHlink,
DHrobot,
DHmatrixA,
DHmatrixT,
DHpositionT,
DHrotationT,
DHtransformationT,
DHlinearjacobian,
DHangularjacobian,
DHjacobian;

local setup, cleanup;

option package,
load = setup; # Module initialization option
unload = cleanup; # Procedure that is called when the module
# is destroyed (i.e. Maple exits)

A.2 type
######## type definitions ########
setup := proc()
# Defines the global variables
global ‘type/link‘, ‘type/robot‘;

# Type definition

41
 ‘type/link‘ := proc(mylink)

local linkparameter;

linkparameter := {
numeric, symbol, name(indexed), function(symbol),
‘&*‘(numeric, {symbol, function(symbol)}),
‘&+‘({numeric, symbol, name(indexed),
function(symbol),
‘&*‘(numeric, {symbol, name(indexed),
function(symbol)})
},
{
numeric, symbol, name(indexed), function(symbol),
‘&*‘(numeric, {symbol, name(indexed),
function(symbol)})})
};

# type::link is a list containing theta, d, a,

# alpha, linktype checks that linktype is of
# allowed specification {p, r}
type(mylink, [linkparameter, linkparameter,
linkparameter, linkparameter, symbol])
and member(mylink[5], {p, r});
end proc;
local linkparameter;

‘type/robot‘ := proc(myrobot)
local svar;
tmp := map(type, myrobot, ’link’);
if member(false, tmp)
# exists elements in myrobot that is not
# type::link
then false
# all elements in myrobot is type::link
else true
end if;
end proc;

userinfo(1, ’DH’, "New types ’link’ and ’robot’ defined.");

NULL

end proc;

cleanup := proc()
global ‘type/link‘, ‘type/robot‘;

userinfo(1, ’DH’, "Cleaning up defined global types.");

42
 ‘type/link‘ := evaln(‘type/link‘); # Eval to a name
‘type/robot‘ := evaln(‘type/robot‘);

NULL

end proc;

A.3 DHparameter
######## DHparameter ########
# Description: Makes a transformation of the DH-parameters of the
# robot if they don’t agree with the "real"
# DH-parameters.
# The procedure is called with:
# DHparameter(theta::Vector[column], theta0::Vector[column],
# thetaTrans::Matrix);
# Input: theta = DH-parameters (type Vector[column])
# theta0 = the displacement from the origin
# (type Vector[column])
# thetaTrans = a 6x6 matrix defining how the DH-parameters
# are related to each other (type Matrix)
# Output: ans = Ttheta*theta + theta0: the transformed DH-
# parameters (type Vector{column])

DHparameter := proc(theta::Vector[column], theta0::Vector[column],

thetaTrans::Matrix)

# Local variables defined

local ans;

# Returned answer
ans := simplify([Link] + theta0);

end proc;

A.4 DHlist2link
######## DHlist2link ########
# Description: Makes a link from the specified DH-parameters and
# jointtype of the link.
# The procedure is called with:
# DHlist2link(theta, d, a, alpha, jointtype)
# Input: theta = DH-parameter theta for the link
# d = DH-parameter d for the link
# a = DH-parameter a for the link
# alpha = DH-parameter alpha for the link
# jointtype = type of joint: revolute (’r’) and
# prismatic (’p’)
# Output: ans = list of the DH-parameters and jointtype.
# ans := [theta d a alpha jointtype]

43
# (type::link)

DHlist2link := proc(theta, d, a, alpha, jointtype)

# Local variables defined

local DHlist, ans;

# Combine the DH-parameters to a list

DHlist := [theta, d, a, alpha, jointtype];

if type(DHlist, ’link’) then ans:= DHlist;

else error " Joint variables not of specified type in type(link)
or joint type not revolute r, prismatic, p.";
end if;

# Returned answer
ans;

end proc;

A.5 DHvariablechange
######## DHvariablechange ########
# Description: Changes the current DH variable of the link to a
# chosen varname.
# The procedure is called with:
# DHvariablechange(link::link, varname::name)
# Input: link = robot link of type::link
# varname = chosen name of DH-variable
# Output: ans = list of DH-parameters and jointtype, with the
# actual DH-variable changed to varname,
# i.e., of type::link

DHvariablechange := proc(link::link, varname::name)

# Local variables defined

local ans;

# Changes the actual DH-variable to the chosen varname

if link[5] = r then # jointtype = revolute(r)

# Numerical value, do not change to varname

# else change theta to varname
ans := [subs(theta = varname, link[1]), op(link[2..5])];

elif link[5] = p then # jointtype = prismatic(p)

# Numerical value, do not change to varname

# else change d to varname
ans := [link[1], subs(d = varname, link[2]), op(link[3..5])];

44
else error "Possible joint type: revolute (r) or prismatic (p).";
end if;

end proc;

A.6 DHlink
######## DHlink ########
# Description: Makes a link from the specified DH-parameters and
# jointtype of the link.
# The procedure is called with:
# DHlink(theta, d, a, alpha, jointtype::symbol, varname::name)
# Input: theta = DH-parameter theta for the link
# d = DH-parameter d for the link
# a = DH-parameter a for the link
# alpha = DH-parameter alpha for the link
# jointtype = type of joint: revolute (’r’) and
# prismatic (’p’)
# varname = chosen name of DH-variable
# Output: ans = list of the DH-parameters and jointtype,
# type::link. The actual DH-variable name is
# changed to the chosen varname.
# ans := [theta d a alpha jointtype]

DHlink := proc(theta, d, a, alpha, jointtype::symbol, varname::name)

# Local variables defined

local templink, link, ans;

# Creating a link from the DH-parameters

# and testing that the created link is of type::link)
templink := DHlist2link(theta, d, a, alpha, jointtype);

# Actual DH-variable changed to the chosen varname

# (changes only if symbolic DH-variables and not numerical ones)
link := DHvariablechange(templink, varname);

# Returned answer
ans := link;

end proc;

A.7 DHrobot
######## DHrobot ########
# Description: Makes a robot from the links.
# The procedure is called with (example with 3 links):
# DHrobot(link1, link2, link3);
# Input: links = the links of type link

45
# Output: ans = list of links; [link1, link2, link3]
# i.e, type::robot

DHrobot := proc()

# Local variables defined

local i, robot, ans;

# Defines the robot

robot := [];

# The links must be of type link

for i from 1 to nargs do
if type(args[i], ’link’) then
robot := [op(robot), args[i]];
else error
"All robot links must be of type ’link’.";
end if;
end do;

# All links collected in a list

ans := robot;

end proc;

A.8 DHmatrixA
######## DHmatrixA ########
# Description: Calculates the Denavit-Hartenberg transformation
# matrix A for the link.
# The procedure is called with:
# DHmatrixA(link);
# Input: link = robot link [theta d a alpha jointtype]
# (type::link)
# Output: ans = Denavit-Hartenberg homogeneous transformation
# matrix A for the link (type: Matrix 4x4)

DHmatrixA := proc(link::link)

# Local variables defined

local theta, d, a, alpha, Rotztheta, Transzd, Transxa, Rotxalpha,
A, ans;

# Picks the DH-parameters for the link object

theta := link[1];
d := link[2];
a := link[3];
alpha := link[4];

# Calculates the transformation matrix A

46
 # Created the rotations/translations
Rotztheta := Matrix([[cos(theta), -sin(theta), 0, 0 ],
[sin(theta), cos(theta), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]]);
Transzd := Matrix([[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, d],
[0, 0, 0, 1]]);
Transxa := Matrix([[1, 0, 0, a],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]]);
Rotxalpha := Matrix([[1, 0, 0, 0],
[0, cos(alpha), -sin(alpha), 0],
[0, sin(alpha), cos(alpha), 0],
[0, 0, 0, 1]]);

# Computes the total transformation consisting of the four

# rotations/translations
A := [Link];

# Returned answer
ans := A;

end proc;

A.9 DHmatrixT
######## DHmatrixT ########
# Description: Calculates the Denavit-Hartenberg transformation
# matrix T between the coordinate frames
# specified by the robot.
# The procedure is called with:
# T := DHmatrixT(robot::robot);
# Input: robot = robot, consisting of links (type::robot)
# Output: ans = Denavit-Hartenberg homogeneous transformation
# matrices Ti
# (table with elements of type: Matrix 4x4)

DHmatrixT := proc(robot::robot)

# Local variables defined

local i, link, A, T, ans;

# Temporary matrix T[0]

# to be able to use T[i-1] for all links (even for i=1)
T[0] := Matrix([[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],

47
 [0, 0, 0, 1]]);

# Generate the transformation matrices T for all 6 links

for i from 1 to nops(robot) do

link[i] := op(i, robot);
A[i] := DHmatrixA(link[i]);
T[i] := T[i-1].A[i]:
end do;

ans := T;

end proc;

A.10 DHpositionT
######## DHpositionT ########
# Description: Calculates the position of the tip of the link
# relative to the chosen "base" coordinate frame
# given by the transformation matrix T.
# The procedure is called with:
# DHpositionT(T::Matrix);
# Input: T = transformation matrix from coordinate frame j to
# coordinate frame i (type: Matrix 4x4)
# Output: ans = position of the tip of the link relative to the
# chosen "base" coordinate frame
# (type: Vector[column] 3x1)

DHpositionT := proc(T::Matrix)

# Local variables defined

local dim, ans;

# Required dimension of matrix T

dim := 4,4;

if op(1, T) = dim
then
# The position of the tip of the link relative to
# coordinate frame i
ans := T[1..3,4];

else error "Matrix dimension must be 4,4."

end if;

end proc;

48
A.11 DHrotationT
######## DHrotationT ########
# Description: Calculates the rotation matrix from coordinate
# frame j to coordinate frame i given by the
# transformation matrix T.
# The procedure is called with:
# DHrotationT(T::Matrix);
# Input: T = transformation matrix from coordinate frame j to
# coordinate frame i (type: Matrix 4x4)
# Output: ans = rotation matrix from coordinate frame j to
# coordinate frame i (type: Matrix 3x3)

DHrotationT := proc(T::Matrix)

# Local variables defined

local dim, ans;

# Required dimension of matrix T

dim := 4,4;

if op(1, T) = dim
then
# The rotation matrix from the transformation matrix T
ans := T[1..3,1..3];

else error "Matrix dimension must be 4,4."

end if;

end proc;

A.12 DHtransformationT
######## DHtransformationT ########
# Description: Transforms the vector p from coordinate frame j to
# coordinate frame i given by the transformation
# matrix T.
# The procedure is called with:
# DHtransformationT(p::Vector[column], T::Matrix)
# Input: p = vector in coordinate frame j
# (type: Vector[column] 3x1)
# T = transformation matrix from coordinate frame j to
# coordinate frame i (type: Matrix 4x4)
# Output: ans = vector p rotatated and translated from coordinate
# frame j to coordinate frame i
# (type: Vector[column] 3x1)

DHtransformationT := proc(p::Vector[column], T::Matrix)

# Local variables defined

49
local d, dim, R, ans;

# Required dimension for p

dim := 3;

if op(1, p) = dim
then
# The position of the tip of the link relative to
# coordinate frame i
d := DHpositionT(T);

# The rotation matrix from the transformation matrix T

R := DHrotationT(T);

# Transforms the vector p according to p_i = R*p + d

ans := R.p + d;

else error "Vector dimension must be 3."

end if;

end proc;

A.13 DHlinearjacobian
######## DHlinearjacobian ########
# Description: Calculates the linear part of the Jacobian,
# describing the relation between the linear velocity
# of the tool position to the joint velocities.
# The procedure is called with:
# DHlinearjacobian(robot::robot);
# Input: robot = robot, consisting of links (type::robot)
# Output: Jv = linear part of the Jacobian for the given number
# of joints in the robot

DHlinearjacobian := proc(robot::robot)

local d, i, Jv, n, R, T, z, zunit, ans;

# Defines the unit vector zunit in coordinate frame i-1

zunit := Vector[column]([0, 0, 1]);

z[0] := zunit;

# Defines the unit matrix R0

R[0] := Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]]);

# Defines d0
d[0] := Vector[column]([0, 0, 0]);

# Calculates the transformation matrices T_i

50
T := DHmatrixT(robot);

# Help variable
n := nops(robot);

# Creates the Jv-matrix of needed size

Jv := Matrix(3, n, 0);

# Calculates R_i, z_i

for i from 2 to n do
R[i-1] := DHrotationT(T[i-1]);
z[i-1] := R[i-1].zunit;
end do;

# Calculates d_i
for i from 1 to n do
d[i] := DHpositionT(T[i]);
end do;

# Calculates the linear part of the Jacobian

for i from 1 to n do
# Test if revolute or prismatic joint
if op(5, op(i, robot)) = p then # prismatic joint
Jv[1..3, i] := z[i-1];
elif op(5, op(i, robot)) = r then # revolute joint
Jv[1..3, i] := CrossProduct(z[i-1], (d[n] - d[i-1]));
end if;
end do;

ans := Jv;

end proc;

A.14 DHangularjacobian
######## DHangularjacobian ########
# Description: Calculates the angular part of the Jacobian,
# describing the relation between the angular velocity
# of the tool position to the joint velocities.
# The function is called with:
# DHangularjacobian(robot::robot);
# Input: robot = robot, consisting of links (type::robot)
# Output: Jomega = angular part of the Jacobian for the given number
# of joints in the robot

DHangularjacobian := proc(robot::robot)

local i, Jomega, n, R, T, z, zunit, ans;

# Defines the unit vector zunit in coordinate frame i-1

51
zunit := Vector[column]([0, 0, 1]);

# Defines the unit matrix R0

R[0] := Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]]);

# Calculates the transformation matrices T_i

T := DHmatrixT(robot);

# Help variable
n := nops(robot);

# Creates the Jomega-matrix of needed size

Jomega := Matrix(3, n, 0);

# Calculates the angular part of the Jacobian

for i from 1 to n do

# Test if revolute or prismatic joint

if op(5, op(i, robot)) = p then # prismatic joint

Jomega[1..3, i] := Vector[column]([0, 0, 0]);

elif op(5, op(i, robot)) = r then # revolute joint

if i = 1 then
z[i-1] := zunit;
else
R[i-1] := DHrotationT(T[i-1]);
z[i-1] := R[i-1].zunit;
end if;

Jomega[1..3, i] := z[i-1];

end if;

end do;

ans := Jomega;

end proc;

A.15 DHjacobian
######## DHjacobian ########
# Description: Calculates the Jacobian, describing the relation
# between the linear and angular velocity of the tool
# position to the joint velocities.
# The function is called with:
# DHjacobian(robot::robot);
# Input: robot = robot, consisting of links (type::robot)

52
# Output: J = Jacobian for the given number of joints of
the robot

DHjacobian := proc(robot::robot)

local J, Jomega, Jv, ans;

# Linear part of the Jacobian

Jv := DHlinearjacobian(robot);

# Angular part of the Jacobian

Jomega := DHangularjacobian(robot);

# Resulting Jacobian
J := Matrix([[Jv], [Jomega]]);

ans := J;

end proc;

53
B Malpe implementation, another way to do it
B.1 A more direct approach
It may be interesting to compare the DH-toolbox in this report to the calculation
of the kinematics of the Stanford manipulator, presented in [11], pages 435–441.
In [11] has used the linalg package, whereas the LinearAlgebra package has
been used here in this report. The linalg package is older and therefore the
procedures used can be slightly different. Also, when treating the problem in a
different way, other procedures may be used. Another difference is that the work
in this report tries to collect the procedures used into user-friendly procedures,
while [11] just calculates the steps one by one and shows the principle without
doing it by means of own procedures.
[11] first by means of alias and seq calculates the homogeneous transfor-
mations Ai , which is presented in Section 5.2. Then these transformations are
multiplied with evalm in order to get the transformation matrix T6 from which
the position and orientation of the tip of the manipulator can be described.
Thereafter the Jacobian is calculated. First the Ti -matrices is splitted into a
translational part and rotational part by means of subvector and submatrix.
The linear velocity part of the Jacobian, described in Section 7.2, is calculated
with jacobian. The angular part, see Section 7.3, is derived by first mapping
the diff procedure to all matrix coefficients, then use transpose, evalm and
simplify. Then the result of the angular Jacobian is concatenated into a sub-
matrix by concat or augment. The linear and angular part of the Jacobian are
then placed below each other in the Jacobian matrix with stack. Then the rank
of the linear part of the Jacobian is discussed. If this does not have maximum
rank (for some kinematic parameters), the robot is said to be in a singular state,
which is a forbidden configuration of the robot arm. By means of subs, det
and addrow some singular states are found. [11]

54