Technical report from Automatic Control at Linköpings universitet

On robot modelling using Maple

Johanna Wallén

Division of Automatic Control

E-mail: johanna@isy.liu.se

9th August 2007

Report no.: LiTH-ISY-R-2723

Address:Department of Electrical EngineeringLinköpings universitetSE-581 83 Linköping, Sweden

WWW: http://www.control.isy.liu.se

AUTOMATIC CONTROLREGLERTEKNIK

LINKÖPINGS UNIVERSITET

Technical reports from the Automatic Control group in Linköping are available fromhttp://www.control.isy.liu.se/publications.

AbstractThis report studies robot modelling by means of the computer algebra toolMaple. First coordinate systems are described, and the more general waywith transformation matrices is chosen in the further work. The positionkinematics of the robot are then described by homogeneous transformations.The Denavit-Hartenberg representation is used, which is a systematic wayto develop the forward kinematics for rigid robots. The velocity kinematicsis then described by the Jacobian. The industrial robot IRB1400 from ABBRobotics is used as an example of the theory and to show the use of theprocedures developed in the Maple programming language.

Keywords: Maple, industrial robot, homogeneous transformation,Denavit-Hartenberg representation, forward kinematics, Jabcobian

Avdelning, InstitutionDivision, Department

Division of Automatic ControlDepartment of Electrical Engineering

DatumDate

2007-08-09

SpråkLanguage

� Svenska/Swedish

� Engelska/English

�

�

RapporttypReport category

� Licentiatavhandling

� Examensarbete

� C-uppsats

� D-uppsats

� Övrig rapport

�

�

URL för elektronisk version

http://www.control.isy.liu.se

ISBN

—

ISRN

—

Serietitel och serienummerTitle of series, numbering

ISSN

1400-3902

LiTH-ISY-R-2723

TitelTitle

On robot modelling using Maple

FörfattareAuthor

Johanna Wallén

SammanfattningAbstract

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

NyckelordKeywords Maple, industrial robot, homogeneous transformation, Denavit-Hartenberg representation,

forward kinematics, Jabcobian

Contents1 Introduction 4

2 Related research 42.1 The chosen models . . . . . . . . . . . . . . . . . . . . . . . . . . 42.2 Why Maple? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

3 Basics about robots and coordinate systems 53.1 Common parts of the industrial robot . . . . . . . . . . . . . . . 53.2 Pairs and joints . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53.3 Coordinate systems . . . . . . . . . . . . . . . . . . . . . . . . . . 6

4 Position kinematics 84.1 Translation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94.2 Rotation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94.3 Different representations of rotations . . . . . . . . . . . . . . . . 104.4 Rigid motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114.5 Differentiation of the transformation matrix . . . . . . . . . . . . 12

5 Denavit-Hartenberg representation 135.1 Homogeneous matrix . . . . . . . . . . . . . . . . . . . . . . . . . 135.2 Standard Denavit-Hartenberg representation . . . . . . . . . . . 14

6 D-H representation – a robot example 156.1 Link parameters and joint variables . . . . . . . . . . . . . . . . . 16

6.1.1 Configuration of IRB1400 . . . . . . . . . . . . . . . . . . 176.1.2 Transformed joint variables . . . . . . . . . . . . . . . . . 176.1.3 Mathematical summary . . . . . . . . . . . . . . . . . . . 186.1.4 Thoughts about the closed kinematic chain . . . . . . . . 186.1.5 How to do in Maple . . . . . . . . . . . . . . . . . . . . . 19

6.2 Transformation matrices . . . . . . . . . . . . . . . . . . . . . . . 236.2.1 How to do in Maple . . . . . . . . . . . . . . . . . . . . . 25

6.3 Rigid motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 266.3.1 How to do in Maple . . . . . . . . . . . . . . . . . . . . . 27

7 Velocity kinematics 277.1 Manipulator Jacobian . . . . . . . . . . . . . . . . . . . . . . . . 287.2 Linear velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

7.2.1 How to do in Maple . . . . . . . . . . . . . . . . . . . . . 307.3 Angular velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

7.3.1 How to do in Maple . . . . . . . . . . . . . . . . . . . . . 327.4 Resulting Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . 33

7.4.1 How to do in Maple . . . . . . . . . . . . . . . . . . . . . 347.5 Discussion about the Jacobian . . . . . . . . . . . . . . . . . . . 34

7.5.1 Interpretation of the Jacobian . . . . . . . . . . . . . . . . 357.5.2 Singularities . . . . . . . . . . . . . . . . . . . . . . . . . . 35

8 Acceleration equations 36

9 Summary of example IRB1400 in Maple 379.1 Step by step . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

10 Concluding remarks and future work 39

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

B Malpe implementation, another way to do it 54B.1 A more direct approach . . . . . . . . . . . . . . . . . . . . . . . 54

3

1 IntroductionIn this report a first step towards making a toolbox for industrial robot modellingin the computer algebra tool Maple [13] is taken. The aim with this projectis not to build a comprehensive and complex toolbox, but to study relativelysimple robot models in order to understand the techniques.

The work leads towards a modelling platform for efficient derivation of thenecessary equations that in the future can aid e.g., developing different modelrepresentations of robot structures with or without mechanical flexibilities orperforming sensor fusion. Additional information from sensors demands signalprocessing and sensor fusion, which is hard to achieve without good models andgood modelling tools. The idea is to provide a platform where it is simpler toevaluate different kinds of sensors and also various sensor locations using Mapleas a tool to symbolically generate the kinematic models. Thereafter the modelswill be incorporated in Matlab- or C-code to implement, e.g., state estimationusing an Extended Kalman Filter (EKF) algorithm.

The numerical calculations described in this report can also be made bymeans of Robotic Toolbox in Matlab [4], but the symbolic manipulations can-not be performed in Robotics Toolbox.

2 Related researchAn example of previous work in this area is [3], where a Mathematica basedapproach to robot modelling is presented by Bienkowski and Kozlowski. Thekinematic 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 adescription). A fairly well-known contribution is Robotics Toolbox [4], developedby Corke, where numerically calculations of kinematics, dynamics and trajectoryplanning are possible. It provides simulation and also analysis of results fromreal robot experiments. Another contribution by Corke is [6], but the focus thereis on the dynamics and how dynamic models of robots can be simplified usingMaple. 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. Althoughsimilar to what in presented in this report, the package Robotica is no longersupported or updated and it is not sure that it works in the last version ofMathematica without editing the source code (according to the homepage ofMark W. Spong, one of the authors of Robotica).

2.1 The chosen modelsThe work in this report can be done for all axis and the model is without jointflexibilities. The examples in this report show the results for just the two orthree first axis of the robot model IRB1400 [16] from ABB Robotics, since it ishard to take in the whole picture because of the size of the expressions for allsix 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 itis 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 reportthe implemented toolbox is called DH-toolbox.

3 Basics about robots and coordinate systemsIt is needed to define suitable coordinate systems for the following work, butfirst of all it is necessary to explain common terms for the different parts usedin the industrial robot modelling.

3.1 Common parts of the industrial robotThe 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 therobot is defined as link 0. It also has n joints. Most of the industrial robotshave six degrees of freedom, which makes it possible to control both the positionof the tool and its orientation. A systematic way to calculate the number ofdegrees of freedom of a robot by means of the so-called Grübler or Kutzbachcriterion is shown in [18].

The dynamics of the robot is coupled and therefore movements of one jointwill 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 betweenthe 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 coordianteframe 3 for every movement of the robot links. See for example Figure 3-1, page64, in [17] for an illustration of these notions.

The assumption of all links as rigid bodies makes the study of robots mucheasier 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 rigidbodies. Also two links that are connected to each other in such a way that norelative motion can occur between them can, from a kinematic point of wiev,be considered as a single link. [18]

3.2 Pairs and jointsThe ith joint has a variable, qi, that represents the relative displacement betweenadjacent links, i.e., each joint has one degree of freedom. The total degrees offreedom for the robot is however equal to the degrees of freedom associated withthe moving links minus the number of contraints imposed by the joints. [18]

The kind of relative motion between the links, connected by the joint, isdetermined 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 witheach other by a substantial surface area it is called a lower pair. Otherwise, if thelinks 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 (calledsliding pair), cylindrical joint, helical joint (called screw pair), spherical jointand plane pair. The frequently used universal joint is a combination of twointersecting revolute joints. Higher pairs are gear pair and cam pair. More aboutthese joints and pairs and their corresponding number of degrees of freedom canbe found in [18]. In this report it is sufficient to know that a revolute joint hasa cylindrical shape where the motion is a rotation with angle θ. On the otherhand, the prismatic joint can be described by a cube with side d and its motionis 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 revoluteor rotational joint, the variable qi is the angle of rotation between the links. Inthe case of prismatic or sliding joint, the variable is the link extension. This isdescribed in more detail in Section 5.2.

The joint variables q1, . . . , qn form a set of generalised coordinates for ann-link robot. Figure 1 shows the joint variables qi for IRB1400 from ABBRobotics, which only has revolute joints. [4, 17]

q1

q2

q3q4

q5

q6

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 revoluteor prismatic joints (both having one degree of freedom), and further work istherefore only applied to revolute or prismatic joints.

3.3 Coordinate systemsThe simplified robot model presented in Figure 2 with two arms in the groundplane has no gravitational force. One way to represent the two axis is to have acoordinate system with the unit vectors r1 and θ1 for the first arm and the unitvectors r2 and θ2 for the following arm [2]

6

x

y

r1

θ1

q1

r2

θ2

q2

Figure 2: Simplified robot model with two arms in the ground plane. Thefigure comes from [2] and shows the global coordinates x and y and the localcoordinate systems with the unit vectors ri, θi associated with the two arms.

r1 = cos(q1)x+ sin(q1)y (1)

θ1 = − sin(q1)x+ cos(q1)y (2)

r2 = cos(q2)r1 + sin(q2)θ1 (3)

θ2 = − sin(q2)r1 + cos(q2)θ1 (4)

Neccessary information about the orientation is actually covered by the unitvectors r1 and r2. The unit vectors θ1 and θ2 can for example be seen as arotation by an angle π

2 from the r1- and r2-vectors. With this in mind it iseasier to understand and verify the derivatives of these unit vectors below

˙r1 = q1θ1 (5)˙θ1 = −q1r1 (6)˙r2 = (q1 + q2)θ2 (7)˙θ2 = −(q1 + q2)r2 (8)

When r1 is differentiated it is only an angular change, because the length ofthe vector r1 (represented by r in the r1-direction) is a constant. This results in˙r1 in only the θ1-direction with the lengt q1 coming from the inner derivative.It is also verified from calculations in [2]. Same discussion can be used whendifferentiating θ1, now with the outlook that θ1 is rotated by an angle π

2 from

r1. It can now easily be seen that ˙θ1 is in the −r1-direction with length q1 from

the inner derivative.

7

Then the derivatives of the unit vectors for the second axis are derived. ˙r2is in the θ2-direction with the length q1 + q2 coming from the inner derivative.˙θ2 is also directed orthogonal to r2, i.e., in the −r2-direction with the lengthfrom the inner derivative.

The second derivates of the unit vectors are derived in [2] and can be depictedas follows

¨r1 = q1θ1 − q21 r1 (9)¨θ1 = −q1r1 − q21 θ1 (10)¨r2 = −(q1 + q2)2r2 + (q1 + q2)θ2 (11)¨θ2 = −(q1 + q2)r2 − (q1 + q2)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 ˙r1 = q1θ1, so the first term q1θ1 is inthe θ1-direction because only q1 is differentiated and θ1 is constant. The secondterm with length q21 from the inner derivative of θ1 is in the r1-direction asexpected, because it is a derivative of θ1. The other derivatives can be verifiedintuitively following a similar discussion.

In this section only the first two axes are described by these coordinatesystems, but it is straight forward to move on and describe the other axes ina similar manner. The trick is to see that all necessary information about theorientation 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 difficultto perform in Maple than expected. It is also more specific than the generalhomogeneous transformations, where the rotations and transformations betweenthe coordinate systems are represented by a single matrix. These homogenoustransformations are therefore used in the further work.

4 Position kinematicsKinematics describes the movements of bodies without considerations of thecause. Position, velocity, acceleration and higher derivatives of the positionvariables are studied. Robot kinematics especially studies how various linksmove with respect to each other and with time [4]. Dynamics on the other handdescribes the movements with respect to also the forces that caused the motion.

There are two ways to describe the position kinematics. Forward kinematicsis one way where the idea of kinematic chains is used and the position andorientation 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 connectedtogether 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 otherway is inverse kinematics which means to calculate the joint configuration ofthe robot from the position and orientation of the robot tool. This is in generala 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 TranslationFirst a parallel translation of the vector p1 by the vector d1

0 in coordinate frame 0is described. It can shortly be written as

p0 = p1 + d10 (13)

4.2 RotationThe transformation between coordinate frame 0 and frame 1 is described andthen frame 2 is added to show the structure of the combined transformationseven more.

The basic rotation matrix R01 describes the transformation of the vector p

between frame 0 and frame 1 as

p0 = R10p1 (14)

The matrix R10 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. R1

0 can thus be given by

R10 =

x1 · x0 y1 · x0 z1 · x0

x1 · y0 y1 · y0 z1 · y0x1 · z0 y1 · z0 z1 · z0

(15)

For example a rotation with an angle θ around the z-axis in the base frame canbe described with the basic rotation matrix

R10 = Rz,θ =

cos θ − sin θ 0

sin θ cos θ 0

0 0 1

(16)

Since the scalar product is commutative, the matrix is orthogonal and the inversetransformation p1 = R0

1p0 is given by

R01 = (R1

0)−1 = (R1

0)T (17)

Now coordinate frame 2 is added, which is related to the previous frame 0and frame 1 as follows.

p0 = R10p1 (18)

p0 = R20p2 (19)

p1 = R21p2 (20)

Combining the rotation matrices gives the transformation from frame 2 toframe 0 according to

p0 = R10R

21p2 = R2

0p2 (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 R1

0 followedby rotating the frame 2 relative to frame 1 according to R2

1. The order of the

9

transformation matrices cannot be changed, because then the interpretationand the result of these rotations are quite different. Rotation is unlike trans-lation a matrix quantity and therefore the rotational transformations do notcommute. [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. Thisis performed by multiplying the transformation matrices in the reverse ordercompared to (21), giving the rotation matrix

R20 = R2

1R10 (22)

4.3 Different representations of rotationsThe elements in the rotation matrix R are not independent. A rigid body canat most have three rotational degrees of freedom, and thus at most three in-dependent variables are needed to specify the orientation. There are differentrepresentations, 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 rotationmatrix always can be represented by a single rotation about a single axis in spaceby 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 angleof rotation about the axis. The equation is therefore called the axis/anglerepresentation of R. The representation is however not unique, since a rotationby the angle −θ about −k gives the same result.

Euler angles The method Euler angles is a more common way to specify therotation matrix R. The Euler angles θ, φ and Ψ are obtained by the followingsuccessive rotations. For example the orientation of frame 1 relative to frame 0are specified by first rotating about the z0-axis by the angle φ. Then rotateabout the current y-axis by the angle θ and last but not least rotate aboutthe current z-axis (which now is the z1-axis) by the angle Ψ. The resultingrotational transformation R1

0 can be described by the product

R10 = 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 productof successive rotations about the principal coordinate axes x0, y0 and z0 in aspecified order. The roll ψ, pitch θ and yaw Ψ angles can be seen in Figure 2-8in [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

R10 = 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 inthis very short description of quaternions.

They can be used to compactly describe an arbitrary rotation. Two vectorsp0 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. Bymeans of quaternions, this expression can be rewritten as

q =

(cos(θ/2)

k sin(θ/2)

)(28)

p0 = q ∗ p1 ∗ q (29)

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

4.4 Rigid motionThe most general move between the coordinate frames 0 and n can be describedby a pure rotation combined with a pure translation. This combination is calleda rigid motion if the rotation matrix Rn0 in the equation

p0 = Rn0 pn + dn0 (30)

is orthogonal, i.e., (Rn0 )TRn0 = I. Important properties of the rotation matrixR worth knowing are det(R) = 1 and the set of all 3×3 rotational matrices arecollected 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 theform

H =

(R d

0 1

)(31)

Since R is orthogonal, the inverse transformation H−1 is defined by

H−1 =

(RT −RT d0 1

)(32)

These transformations are called homogeneous transformations. To be able towrite the transformation as a matrix multiplication, the homogeneous represen-

11

tation Pi of the vector pi is defined as

Pi =

(pi

1

)(33)

(34)

Now the transformation (30) can be written as the homogeneous matrix equation

P0 = H10P1 (35)

Combining two homogeneous transformations, e.g., p0 = R10p1 + d1

0 andp1 = R2

1p2 + d21, gives

H10H

21 =

(R1

0 d10

0 1

)(R2

1 d21

0 1

)=

(R1

0R21 R1

0d21 + d1

0

0 1

)(36)

4.5 Differentiation of the transformation matrixA skew symmetric matrix S is defined as

ST + S = 0 (37)

If the rotation matrix R(θ) is dependent of the variable θ, the derivative can bedescribed by means of the skew symmetric matrix S

dR

dθ= SR(θ) (38)

In this report the rotation matrix R(q) depends on the joint variables q. In thesame way, a time varying matrix R(t) is described as

R(t) = S(t)R(t) (39)

with the skew symmetric rotation matrix S(t). Since S(t) is skew symmetric, itcan 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ωn0 for the tool expressed in the base frame 0. It then gives

Rn0 (t) = S(ωn0 (t)

)Rn0 (t) (40)

which, after reordering of the terms, gives

S(ωn0 (t)

)= Rn0 (t)Rn0 (t)−1 = Rn0 (t)Rn0 (t)T (41)

If ω(t) =(ω1(t) ω2(t) ω3(t)

), the skew symmetric matrix S

(ω(t)

)then is defined

S(ω(t)

)=

0 −ω3(t) ω2(t)

ω3(t) 0 −ω1(t)

−ω2(t) ω1(t) 0

(42)

With these skew symmetric matrices it is possible to simplify many of the com-putations when deriving the relative velocity and acceleration transformationsbetween coordinate frames. How to use the properties of the skew symmet-ric matrices in order to simplify the differentiation of the Jacobian is furtherdescribed in Section 8. [17]

12

5 Denavit-Hartenberg representationThe 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 commonlyused in robotic applications.

There are two slightly different approaches to the convention, the so calledstandard D-H notation described in for example [17] and the modified D-H formfound 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 iin the modified form. The expressions for the link transformation matricesare different, but both notations represent a joint as two translations and tworotations.

5.1 Homogeneous matrixThe 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 − 1under the assumption that the joints are either revolute or prismatic. Thetransformation matrix T ji that transforms the coordinates of a point expressedin frame j to frame i can then be written as

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

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

Ai =

(Rii−1 dii−1

0 1

)(44)

The total homogeneous transform T ji is then given by

T ji = Ai+1 · · ·Aj =

(Rji dji

0 1

), i < j (45)

whereRji = Ri+1

i · · ·Rj−1j , i < j (46)

and dj−1i is given recursively by

dji = dj−1i +Rj−1

i dj−1j , i < j (47)

With i = 0 and j = n, (45) gives the homogeneous matrix Tn0 which transformsthe coordinates from frame n to frame 0

Tn0 =

(Rn0 dn0

0 1

)(48)

The position and orientation of the robot tool in the inertial coordinate frame 0are thereby described by

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

13

which uses the current joint variable qi in every transformation step on the wayfrom frame n to frame 0. This results in the transformation of the vector pnexpressed in frame n to frame 0 as in the relation

pn0 = Rn0 pn + dn0 (50)

5.2 Standard Denavit-Hartenberg representationEach homogeneous transformation Ai is in the D-H representation regarded asa product of four basic transformations

Ai = Rotz,θiTransz,diTransx,aiRotx,αi (51)

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

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

• Length ai: distance from the origin oi (frame i) to the intersection betweenxi- 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 normalto xi. [17]

When performing the translations di, ai and the rotations θi, αi in (51) itis important that they are applied in right order. First a rotation θi aroundthe z-axis is performed, then a translation di along the new z-axis followed by atranslation ai along the new x-axis and last but not least a rotation αi aroundthe new x-axis. See for example Figure 3-4, page 69, in [17] for an illustrationof the transformations.

Under the circumstances that the origin oi (frame 0) do not need to lie atjoint i, the coordinate frames 0, . . . , n for the robot can always be chosen in sucha way that the following two conditions are satisfied [17]

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

(D-H2) xi intersects zi−1

(52)

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

14

With the four basic transformations, the homogeneous transformation Aithen can be mathematically written as

Ai = Rotz,θiTransz,diTransx,aiRotx,αi

=

cos θi − sin θi 0 0

sin θi cos θi 0 0

0 0 1 0

0 0 0 1

1 0 0 0

0 1 0 0

0 0 1 di

0 0 0 1

1 0 0 ai

0 1 0 0

0 0 1 0

0 0 0 1

1 0 0 0

0 cosαi − sinαi 0

0 sinαi cosαi 0

0 0 0 1

=

cos θi − sin θi cosαi sin θi sinαi ai cos θisin θ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

Ai =

(Rii−1 dii−1

0 1

)It can directly be seen that

Rii−1 =

cos θi − sin θi cosαi sin θi sinαisin θi cos θi cosαi − cos θi sinαi

0 sinαi cosαi

(54)

dii−1 =

ai cos θiai sin θidi

(55)

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 aprismatic joint, see also Section 5.1 for a discussion about different types of linksand joint variables. [17]

One important thing to notice is however that choices of the coordinateframes are not unique. [17]

6 D-H representation – a robot exampleThis 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 describedhow these variables and matrices are treated in the DH-toolbox implementedin the Maple programming language.

Figure 3: The industrial robot IRB1400 with a payload of 5 kg and a maximumrange of 1.44 m. Picture from ABB Robotics public archive [1].

6.1 Link parameters and joint variablesThe D-H link parameters for IRB1400 are given in Table 1 and the robot ispositioned 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 tobe known before the velocity kinematics of the robot can be derived. Let ussay that the joint variables for this robot is, generally speaking, the variables φiaccording to

φ =(φ1 φ2 φ3 φ4 φ5 φ6

)T (56)

These joint variables can be determined by reasoning around the movement ofthe actual robot link and then verified by a sketch of the coordinate frames forthe 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, andthe 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 foreach link of the robot IRB1400, this gives that all joints are revolute and thejoint variable φi is equal to the joint angle θi for each joint, see Section 5.2 fora description of the D-H representation and the variables involved.

But, are these variables the “real” D-H joint variables for the robot? Or dowe 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 anopen kinematic chain and those who have a closed kinematic chain. Kinematicchains is described in Section 4 and the robot is now seen as a set of rigidlinks connected together by different joints, under the assumption that eachjoint only has a single degree of freedom. In Section 3.2 we discussed differenttypes of joints and established that all types of joints can be described by onlyrevolute or prismatic joints with one degree of freedom each, which satisfies thisassumption.

The chain of robot links can be open, with every link connected to everyother link by one and only one path. When the kinematic chain is connected toevery other link by at least two distinct paths, it forms two or more closed-loopchains and therefore is a closed kinematic chain. [18]

The robot IRB1400 has, as can be seen in Figure 1 or Figure 3, a mechanicalcoupling 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 isalso called a parallelogram linkage structure to link 3 [17]. In practice thisparallelogram linkage structure means that joint 2 and joint 3 are coupled toeach other. If motor 2 is moved, but not motor 3, the angle between the upperarm (x3-axis) and zo-axis is constant, so that link 3 keeps the same orientationwith respect to the base frame, i.e., when moving motor 2 it will affect boththe variables θ2 and θ3. The profitable idea with this parallelogram linkagestructure is that motor 3, which moves axis 3, is placed below this axis. Theupper robot links can then be made thinner and stiffer and it is easier andcheaper to move the robot links. It also reduces the production cost sinceless material is needed. A drawback with this structure is however that themovement of the robot is more restricted and the robot cannot rotate the upperlinks with an angle π (called bending backwards) — instead the whole robothas 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 doesnot 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 correspondingD-H joint variables for the transformed, open structure.

The need of transforming the joint variables to “real” D-H variables (alsocalled 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 certainposition and orientation (pose). (In this case it by the way happens to be aposition that the robot never can reach.) This “zero variable pose” is howeverdifferent from the pose of the robot when its joint angles are zero, so we needto transform the variables in order to achieve an agreement between the zerorobot 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 jointangles φi need to be translated from its origin with the vector θ0 and trans-formed by the matrix θTrans. Generally this gives the expression for the D-Hparameters θ [16] as follows

θ = θTransφ+ θ0 (57)

This can be performed in Maple by DHparameter, see also Section A.3. Forthe IRB1400 case with mechanical coupling between joint 2 and joint 3 and thezero pose given in Table 1, it results in

θTrans =

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

and θ0 =

0

−π/20

0

0

π

(58)

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 asgeneral variables used and differentiated in for example Section 7 and Section 8and 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 mentionedin 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 inTable 2.

6.1.4 Thoughts about the closed kinematic chain

In this report we have investigated certain types of closed kinematic chains thatcan be described on a bilinear form. The parameter transformation in (57) isperformed at the beginning, i.e., when determining the D-H link parametersand joint variables. Another approach may be to calculate the kinematic re-lations using the joint variables for the actual robot and afterwards transformthe result into the “real D-H” joint variables for the corresponding, transformedopen kinematic chain.

A reasonable idea in the approach with the transformation afterwards isthat 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 therobot 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 doesnot 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 beentreated 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 wordsadded before any kinematic or dynamic functions are operating on the jointangles. 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 isa need for an offset; when the joint angles are zero the robot may be in anunachievable pose. The offset then makes it possible to perform the pose of therobot 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 togetherto 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 typesof inputs. This facilitates a lot if one know how Maple thinks about typesand operands. Therefore this section begins with a try to summarize the mostimportant features of types and operands in Maple, followed by a descriptionof 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. Thefirst operand in θ(t) is θ, but in θ1(t), we have to investigate the first operand ofthe 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 inMaple 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 sumof 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. −π

2is in the same way a multiplication (− 1

2 )∗π with operands of types fraction (ornumeric, 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 (typescombined of defined Maple types) that are interesting as arguments for thelinks are collected in Table 3 and are discussed further on when creating thelinks 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)−θ2(t)

’&+’(function(symbol), ’&*’(numeric, function(symbol)))

−θ(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 isnot 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 packageVectorCalculus. 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 specialoperator in a certain package is invoked by package:-operator. A simpleexample is LinearAlgebra:-Transpose(MyMatrix).

During the tests with types in Maple, we have found out the followingstructure of types. The type numeric is a larger type class, containing thesubclasses 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 definedin (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 θTrans 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 ofthe function DHparameter, see Section A.3.

DHparameter(DHtheta, DHtheta0, thetaTrans);

21

The resulting D-H parameters θ in (57) are

θ =

θ1(t)

θ2(t)− π/2

−θ2(t) + θ3(t)

θ4(t)

θ5(t)

θ6(t) + π

(59)

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-fined by its D-H parameters and type of link; revolute (r) or prismatic (p). Link iis 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 thesecond 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

link1 :=[θ1 d1 a1 α1 p

]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 procedureDHlist2link 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 beseen as a supplement to the table. As is clearly shown, there are lots of differenttypes in Maple that have to collaborate with us...

The expressions in Table 3 can be combined to one structured type, whichconsists of the following types listed below in order to handle mixed orders ofthe 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 chosenvariable name according to following example. In Section 6.1.3 the variable nameq 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 answersDHlink1 :=

[θ1 q1 a1 α1 p

]DHlink2 :=

[q2 d2 a2 α2 r

]of type link, where the D-H joint variable θi is exchanged in the case of arevolute 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-Hvariable. See also Section A.5 for the details. The creation of a link and changeof variable name can also be done directly by using DHlink, see Section A.6 andthe example below.

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

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);

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 ofrobot links with proper characteristics (i.e., of type link) after each other ina list. The procedure DHrobot takes at least one link as argument, and canalso take arbitrarily many links. For example, with link1 and link2 defined asabove a robot can be defined as

robot := DHrobot(link1, link2);

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 matricesFirst, as an example, the homogeneous transformations for the first two links inthe 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.

A1 ≈

1 0 0 0.15

0 0 1 0

0 −1 0 0.475

0 0 0 1

(60)

A2 ≈

0 1 0 0

−1 0 0 −0.6

0 0 1 0

0 0 0 1

(61)

These matrices describe the two coordinate frames associated to the two linksand the way to go between them. T 2

0 then gives the position d20 and orientation

R20 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

T 20 = A1A2 ≈

0 1 0 0.150

0 0 1 0

1 0 0 1.075

0 0 0 1

(62)

R20 =

0 1 0

0 0 1

1 0 0

0 0 0

(63)

d20 =

0.150

0

1.075

1

(64)

T 30 gives the same for frame 3 expressed in the base coordinates (frame 0)

T 30 = A1A2A3 ≈

0 0 1 0.150

0 −1 0 0

1 0 0 1.195

0 0 0 1

(65)

The transformation matrix for the robot tool (frame 6) relative to the baseframe 0 is

T 60 = A1A2A3A4A5A6 ≈

0 0 1 0.955

0 1 0 0

−1 0 0 1.195

0 0 0 1

(66)

24

From this it can be seen that the last column represents the position of therobot tool relative to the base frame, i.e., d6

0 as described in (45)

d60 ≈

0.955

0

1.195

(67)

which means that the position of the robot tool relative to the base frame of therobot is 0.995 m in the x0-direction, 0 m in the y0-direction and 1.195 m in thez0-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 procedureDHmatrixA, see Section A.8. For example, a revolute link with the D-H jointvariable θ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)

A1 =

cos q1 − sin q1 cosα1 sin q1 sinα1 a1 cos q1sin q1 cos q1 cosα1 − cos q1 sinα1 a1 sin q1

0 sinα1 cosα1 d1

0 0 0 1

(68)

With all D-H parameters constant according to Table 1, the first homogeneousmatrix 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 aninteresting feature in Maple, all link parameters are of type integer

link1 := [1, 2, 3, 4, r];A[1] := DHmatrixA(link1);

the result becomes

A1 =

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)

0 sin(4) cos(4) 2

0 0 0 1

(69)

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 numericallyusing floating-point arithmetic.

25

Transformation matrices Now the general transformation matrices T i0 (45)are derived in Maple. The transformation matrices below are given withDHmatrixT, see Section A.9. In this example the robot consists of the firsttwo 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

myrobot :=[ [θ1(t) 0.475 0.15 −π

2 r

],[θ2(t)− π

2 0 0.6 0 r

] ]and the transformation matrix T, which is a table of the transformation matricesT1 and T2. The respective matrices can be viewed by typing the first and secondelement in the table, i.e., T[1] and T[2], which gives

T 10 = A1 =

cos θ1 0 − sin θ1 0.15 cos θ1sin θ1 0 cos θ1 0.15 sin θ1

0 −1 0 0.475

0 0 0 1

(70)

T 20 = A1A2

=

cos θ1 0 − sin θ1 0.15 cos θ1sin θ1 0 cos θ1 0.15 sin θ1

0 −1 0 0.475

0 0 0 1

sin θ2 cos θ2 0 0.6 sin θ2− cos θ2 sin θ2 0 −0.6 cos θ2

0 0 1 0

0 0 0 1

=

cos θ1 sin θ2 cos θ1 cos θ2 − sin θ1 0.6 cos θ1 sin θ2 + 0.15 cos θ1sin θ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 θ20 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 bepresented here (too large!).

6.3 Rigid motionIn the IRB1400 example, we have as previously, the D-H parameters givenaccording to (57) with the parameter values in Table 2. As an example of howthe transformation matrices derived above can be used, we study the vector

p2 =

1

1

1

(72)

26

expressed in coordinate frame 2. It is transformed to the base frame 0 by meansof the transformation matrix

R20 =

cos θ1 sin θ2 cos θ1 cos θ2 − sin θ1sin θ1 sin θ2 sin θ1 cos θ2 cos θ1

cos θ2 − sin θ2 0

(73)

and the vector from the origin to coordinate frame 2

d20 =

0.6 cos θ1 sin θ2 + 0.15 cos θ10.6 sin θ1 sin θ2 + 0.15 sin θ1

0.475 + 0.6 cos θ2

(74)

Using (50) the vector expressed in frame 0 can now be calculated as

p20 = R2

0p2 + d20 (75)

This gives

p20 =

1.6 cos θ1 sin θ2 + cos θ1 cos θ2 − sin θ1 + 0.15 cos θ11.6 sin θ1 sin θ2 + sin θ1 cos θ2 + cos θ1 + 0.15 sin θ1

1.6 cos θ2 − sin θ2 + 0.475

(76)

6.3.1 How to do in Maple

In Maple this can be calculated by the procedures DHpositionT, DHrotationTand DHtransformationT, see Section A.10, Section A.11 and Section A.12 andbelow how to use them. This example uses the first two links with the robot inthe 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 kinematicsThe velocity kinematics relates the linear and angular velocities of a point onthe robot to the joint velocities. The D-H joint variables represent the relativedisplacement between adjoining links. In this section the robot has n links. Theindustrial robot IRB1400 is used to illustrate the theory, and the joint variablesare qi = θi in this case. If nothing else is mentioned, the reference used in thissection is [17].

27

7.1 Manipulator JacobianThe forward kinematic equations derived in Section 5 determine the positionand orientation of the robot tool given the D-H joint variables. The Jacobian ofthis function determines the linear and angular velocities of a point on the robotrelated to the joint velocities and it is one of the most important quantities in theanalysis and control of the robot motion. It is used in many aspects in robotcontrol, like planning and execution of smooth trajectories, determination ofsingular configurations, execution of coordinated motion, derivation of dynamicequations of motion and last but not least to transform the forces and torquesfrom 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 0in (45) is rewritten below for simplicity with D-H joint variables qi

Tn0 (q) =

(Rn0 (q) dn0 (q)

0 1

)where

dn0 = Ri−10 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 T 0

0 (q) = I. This givesaccording to the relation above that R0

0(q) = I and d00(q) = (0 0 0)T .

The angular velocity vector ωn0 is defined as the skew symmetric matrix (seealso Section 4.5)

S(ωn0 ) = Rn0 (Rn0 )T (78)and the linear velocity of the robot tool is

vn0 = dn0 (79)

where dn0 means the time derivative ddn0

dt . Written on the form

vn0 = Jv q (80)ωn0 = Jω q (81)

(80) and (81) can be combined to(vn0

ωn0

)=

(Jv

Jω

)q = Jn0 q (82)

Jn0 is called the Manipulator Jacobian, or just the Jacobian. It is a 6×n-matrixbecause it represents the instantaneous transformation between the n-vector ofjoint velocities and the 6-vector which describes the linear and angular velocitiesof the robot tool. In the following sections it is described how the matrices Jvfor the linear velocities and Jω for the angular velocities can be derived.

7.2 Linear velocityThe linear velocity of the robot tool is described by just dn0 , see (79). Using thechain rule of differentiation the result is

dn0 =n∑i=1

∂dn0∂qi

qi (83)

28

The differentiation (83) combined with (80)gives that the ith column of Jv is∂dn

0∂qi

. This expression is the linear velocity if qi is one and all the other qj arezero, which is achieved if the ith joint is actuated at unit velocity and all theother joints are fixed. There are two cases; a revolute joint and a prismatic jointand the respective joint is described by the relation (77).

Prismatic joint Study (77), repeated here

dn0 = Ri−10 dni−1 + di−1

0

The prismatic joint i gives Rj−10 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.

Ai =

cos θi − sin θi cosαi sin θi sinαi ai cos θisin θi cos θi cosαi − cos θi sinαi ai sin θi

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, andthere are zeros in element A3,1, A4,1, A4,2 and A4,3. When two matrices of thisstructure are multiplied, the di-variables are all in the fourth column, whichdoes not affect the behaviour of the R matrices, which is just the upper left3× 3-matrix.

Between the coordinate frames i and i− 1 we have the relation

dii−1 = Rii−1aix+ diz (84)

which is a transformation from coordinate frame i to frame i− 1, which can bedescribed on the form (30), i.e., pi−1 = Rii−1pi + dii−1. In this case (84) thetranslated and rotated vector results in dii−1, with the translation term diz (di isthe D-H variable called offset) which is the offset from the origin in expressedcordinate frame i − 1 and aix (ai is the D-H variable called lenght) which isthe term to be rotated. The unit vectors x = (1 0 0)T and z = (0 0 1)T areexpressed in frame i− 1.

If all other joints but the ith one are fixed, we have the time derivativesof di equal to zero for all joints except the ith one, i.e., dj = 0, j 6= i. Thedifferentiation of (77) then together with (84) gives

dn0 = Ri−10 dii−1 = diR

i−10 z = dizi−1 (85)

where di is the D-H variable called offset and zi−1 is the resulting vector inthe base frame when the unit vector z = (0 0 1)T in coordinate frame i − 1 isexpressed in the base frame with the transformation matrix Ri−1

0 . Comparedto (83), with qi = di for the prismatic joint, this gives

∂dn0∂qi

= zi−1 = Ri−10 z = Ri−1

0

0

0

1

(86)

29

Revolute joint The revolute joint i is described by the relation (77), repeatedhere for convenience

dn0 = Ri−10 dni−1 + di−1

0

If only the ith joint is actuated, all joint angles except θi are fixed (constant)and di−1

0 and Ri−10 are constant [16]. Differentiation of (77) now gives

dn0 = Ri−10 dni−1 (87)

Since the joint is revolute and therefore the motion of link i is a rotation qi = θiabout the axis zi−1, according to [16] it gives

dni−1 = θiz × dni−1 (88)

with z = (0 0 1)T in coordinate frame i − 1 as usual. When combining theseequations the result, with Ri−1

0 z = zi−1, is

dn0 = Ri−10

(θiz × dni−1

)= θiR

i−10 z ×Ri−1

0 dni−1 = θizi−1 × (dn0 − di−10 ) (89)

The last equality can be explained by the verbal picture that the positon ofthe tool, expressed in the base frame is dno . This is also the position of the linkposition next to the tool expressed in the base frame, plus the position from thissecond last link to the last tool position, i.e., dn0 = di−1

0 + dni−1. Thsi relationcan also be seen in for example Figure 5-1 [17].

From (83), with qi = θi, it can be seen that

∂dn0∂qi

= zi−1 × (dn0 − di−10 ) (90)

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

Jvi =∂dn0∂qi

=

zi−1, prismatic joint

zi−1 × (dn0 − di−10 ), revolute joint

(92)

z0 is of course the unit vector z = (0 0 1)T in the base frame and n is thenumber 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 Mapledescribed 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 Jv2according to (92) gives the relation

Jv2 = z1 × (d60 − d1

0) (93)

where z1 = R10 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 sizesand indices of the z-variable in the workspace.) In Maple this calculation lookslike

30

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

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

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 sameway. The symbolic result is however several (5-6) pages of Maple output,because the transformation matrices T i0 (and thereby di0) grow with increasingindex i. It is desirable to avoid deriving these expressions by hand, and insteaduse a computer algebra tool, e.g., Maple.

In order to be able to show something from an overall picture, the D-Hlink parameters for the robot in zero pose, given in Table 1, is used. Usingthe procedure DHlinearjacobian, see Section A.13, in order to calculated thelinear 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

Jv ≈

0 0.7200 0.1200 0 0 0

0.9550 0 0 0 0 0

0 −0.8050 −0.8050 0 −0.0850 0

(94)

which corresponds to the result in [16].

7.3 Angular velocityAngular velocities can be added vectorially if they all are expressed in a commoncoordinate frame [17]. It is then possible to express the angular velocity of eachlink in the base frame and then sum them to the total angular velocity of therobot tool.

Prismatic joint If for example the ith joint is prismatic, there is a puretranslation and therefore the angular velocity vector is zero according to

ωii−1 = 0, prismatic joint (95)

31

Revolute joint If the ith joint instead is revolute, the joint variable is qi = θiand the z-axis is the axis of rotation. The angular velocity expressed in theframe i− 1 is

ωii−1 = qiz, 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 fromeach link gives the following angular velocity of the robot tool for the robot withn links [17]

ωn0 = ρ1θ1z + ρ2θ2R10z + . . .+ ρnθnR

n−10 z =

n∑i−1

ρiθizi−1 =n∑i−1

ρiqizi−1 (97)

where

ρi =

1, revolute joint

0, prismatic joint(98)

The following equation describes how the unit vector z in coordinate frame i−1is expressed in the base frame by means of the transformation matrix Ri−1

0 asin the case of the linear velocity. The resulting vector in the base frame is thencalled zi−1 according to

zi−1 = Ri−10 z (99)

Comparison between (81) and (97) now gives the lower angular half Jω of theJacobian as

Jω =(ρ1z0 . . . ρnzn−1

), (100)

ρi =

0, prismatic joint,

1, revolute joint.(101)

which is a matrix of the unit vectors z for each coordinate frame transformedinto 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. Whenwe take more than two links into account in the industrial robot model, theexpressions for the Jacobian become very large, because they are functions ofthe 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 therobot IRB1400 are examined in Maple. The D-H variables are here manuallytransformed to “real” D-H variables, this can of course also be done with theprocedure DHparameter, as described in Section 6. This gives the first four linksand 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 = R10z (102)

is in Maple computed as below. Here we use the name zunit instead of z inorder 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

Jω2 =

− sin q1(t)

cos q1(t)

0

(103)

With DHangularjacobian, see Section A.14, the part Jω of the Jacobian for thefour first joints can easily be calculated as

Jomega := DHangularjacobian(myrobot);

with the result

Jω =

0 − sin q1(t) − sin q1(t) Jω14

0 cos q1(t) cos q1(t) Jω24

1 0 0 Jω34

(104)

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 theJacobian, gives

Jω =

0 0 0 1 0 1

0 1 1 0 1 0

1 0 0 0 0 0

(105)

which also corresponds to the result in [16].

7.4 Resulting JacobianIn this section the examples with the linear and angular Jacobian in Mapleare collected and the procedures DHlinearjacobian and DHangularjacobianare combined to DHjacobian, which calculates the whole Jacobian directly. Seealso 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 moreillustrative here. First the links are defined, the robot model is created and thenthe 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

J60 =

(Jv

Jω

)=

0 0.7200 0.1200 0 0 0

0.9550 0 0 0 0 0

0 −0.8050 −0.8050 0 −0.0850 0

0 0 0 1 0 1

0 1 1 0 1 0

1 0 0 0 0 0

(106)

This can be compared to the explicit linear part (94) and angular part (105) ofthe Jacobian, which of course are the same as the linear and angular part of thematrix 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

J60 =

0 0 0 1 0 1

0 1 1 0 1 0

1 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

(107)

As can be seen, the Jacobian now has a simpler structure, because the angularpart now only consists of zeros. The linear part also avoids the more complicatedcross product zi−1 × (dn0 − di−1

0 ), where n is the number of links of the robot.

7.5 Discussion about the JacobianSince the Jacobian is an important quantity in robot modelling, analysis andcontrol, see also Section 7.1, there are of course many interesting things to knowabout it. In this section some of the ideas and thoughts about the Jacobian arediscussed. 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),

X =

(vn0

ωn0

)=

(Jv

Jω

)q = Jn0 q

it can be seen that Jn0 describes how the linear and angular velocities are relatedto the derivatives of the joint variables qi. For example the first element in theupper left corner of Jn0 describes how the first joint is related to the derivativeof the first joint variable q1.

The relation (82) can also be seen as a transformation between the jointvelocities and the linear and angular velocities of the robot tool [17] (the robottool velocities X). Infinitesimally it defines a linear transformation

dX = J(q)dq

between the differentials dq and dX. The first element of vn0 is for example thelinear 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 toinvolve only the links up to that point, i.e., n = ndesired. It is also shown inthe examples with the angular part of the Jacobian, Jω, where it is calculatedfor the first four joints. Then the linear and angular velocity for that positionof 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 inSection 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 thechosen coordinate frames, but the robot configurations themselves are geomet-ric quantities that are independent of the chosen frames. When working withrobots, it is unavoidable to end up in singularities, also called singular configu-rations. At a singular configuration, a vector F in the nullspace of JT does notproduce any torque and therefore also no rotation about the joints. There arealso directions in the cartesian space where the robot cannot exert forces. Thishappens when columns in the Jacobian are linearly dependent and the actualconfiguration q gives a decreasing rank of J . For example a robot with 6 linksgives the Jacobian (82) as a 6 × 6-matrix with configuration q. It is singular ifand only if

det J(q) = 0 (108)

Identifying the singularities is important in robot control, since at singularconfigurations

• 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 jointtorques,

• 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 andorientation of the robot tool, does not exist. Near singularities there may beno solution or infinitely many solutions to the problem [17]. This is a typicalbehaviour in a singularity; we have a configuration where a rotation in a certainjoint can be fully counteracted by rotating another joint in “opposite direction”.

When dealing with robots with spherical wrists, this makes it possible to splitthe determination of the singular configurations into two simpler problems; armsingularities and wrist singularities. Arm singularities are singularities fromthe motion of the arm (the first three or more links) and wrist singularitiesresults from motion of the spherical wrist. As an example of how to split theconfiguration, we study a robot with 6 links. The robot then consists of an armwith three degrees of freedom and a spherical wrist with the remaining threedegrees of freedom.

The wrist singularity only appears when two revolute joints are collinearanywhere, since an equal and opposite rotation about the axes gives no netmotion of the robot tool. This singularity is unavoidable without having me-chanical limits that prevents the axes from being collinear. Arm singularitiesoccur 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 theJacobian relating to the wrist, see [17] for a further discussion.

8 Acceleration equationsDeriving the Jacobian

X =

(xn0

ωn0

)=

(vn0

ωn0

)= Jn0 q (109)

gives the linear and angular acceleration [17] according to

X =

(xn0

ωn0

)=

d

dt

(Jn0 (q)q

)= Jn0 (q)q +

d

dt

(Jn0 (q)

)q (110)

= Jn0 (q)q +( n∑i=1

∂

∂qi

(Jn0 (q)

)qi

)q (111)

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

q =(Jn0 (q)

)−1(X − d

dt

(Jn0 (q)

)q

)(112)

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 etis added. θ is a vector containing the measured joint angles and χ describesthe acceleration vector in the local coordinate frame of the accelerometer. Acomparison to equation (110) shows a need of computing the time derivativesof the Jacobian and considering the complexity of the Jacobian it is clear thatthis derivation is impossible to do by hand. This expression is also used whencomputing 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 thedeveloped procedures in DH-toolbox in Maple, as an overwiev and summaryof the steps in the symbolic calculations. It is then compared to how these stepscan be calculated as concentrated as possible by the procedures.

9.1 Step by stepThe robot has n links and the D-H parameters angle θi, length ai, offset di andtwist αi are assumed to be known for each link of the robot. The numericallink parameters are from Table 1. In order to avoid huge expressions, and showthe calculation steps a bit clearer, the robot in this example is assumed to haven = 2 revolute links. The robot does not have a parallelogram linkage structurewith a closed kinematic chain, which means that the transformation matrixθTrans in (57)

θ = θTransφ+ θ0

is

θTrans =

(1 0

0 1

)However, the parameters have to be translated so the “real” D-H parametersare 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 typeof 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, isreplaced 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 performedby 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 eachlink 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 exampleT2 is needed, just type T2 := T[2]. If we in the following step are interested incalculating the relation p2

0 = R20p2+d2

0 with for example the vector p2 = (1 1 1)T ,the answer p2

0 (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 R20 or the translation vector d2

0 are thesought quantities

R02 := DHrotationT(T[2]);d02 := DHpositionT(T[2]);

Last but not least how to calculate the different parts of the Jacobian (82)(vn0

ωn0

)=

(Jv

Jω

)q = Jn0 q

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 workThe work described in this report is a first step towards a toolbox, calledDH-toolbox, for industrial robot modelling in the computer algebra tool Maple,used for deriving different representations of robot structures with mechanicalflexibilities. In the toolbox it shall be possible to develop nonlinear as well aslinearized state space models and also choose different locations for sensors. Forexample the models shall give a structure that is suitable for example for theSystem Identification Toolbox in Matlab [14].

A realistic ambition is for example to first do all steps for a robot model withtwo axis, first without joint flexibilities and then incorporate them. Thereafterthe same is performed for a robot model with three axis without/with jointflexibilities.

Future work includes to further develop the Maple functions and makea package with a user friendly interface. It also means to develop an exportfunction where the result can be exported in a format that can be read fromMatlab and/or C for including the results, e.g., in an EKF algorithm for stateestimation.

References[1] ABB. Open public archive of robot images. www.abb.se, October 2006.

[2] Daniel Ankelhed and Lars Stenlind. Tillståndsskattning i robotmodell medaccelerometrar. Master’s thesis no LiTH-ISY-EX-05/3650, Department ofElectrical Engineering, Linköpings universitet, Sweden, 2005.

[3] Adam Bienkowski and Krzysztof Kozlowski. A toolbox for teaching andresearching 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 RoboticsToolbox!

[5] Peter I. Corke. Symbolic algebra for manipulator dynamics. Technicalreport, CSIRO Division of Manufacturing Technology, Australia, August1996.

[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 forlower-pair mechanisms based on matrices. ASME Journal of Applied Me-chanics, pages 215–221, June 1955.

[9] Karol Dobrovodsky. Quaternion position representation in robot kinematicstructures. 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 homogeneoustransforms, quaternions, and computational efficiency. IEEE Transactionson 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 flexibleindustrial 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. http://www.maplesoft.com.

[14] The MathWorks. Matlab The Language of Technical Computing. Version7, 2007.

[15] John F. Nethery and Mark W. Spong. Robotica: A Mathematica packagefor 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. JohnWiley & Sons, Ltd, New York, 1989.

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

40

A Malpe implementation

A.1 DH.mpl

# 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-Hartenbergparameters and transformation matrices.";

# exports all proceduresexport

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

local setup, cleanup;

option package,load = setup; # Module initialization optionunload = cleanup; # Procedure that is called when the module

# is destroyed (i.e. Maple exits)

A.2 type

######## type definitions ########setup := proc()

# Defines the global variablesglobal ‘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::linkthen false# all elements in myrobot is type::linkelse 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 definedlocal ans;

# Returned answerans := simplify(thetaTrans.theta + 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 definedlocal DHlist, ans;

# Combine the DH-parameters to a listDHlist := [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 answerans;

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 definedlocal ans;

# Changes the actual DH-variable to the chosen varnameif link[5] = r then # jointtype = revolute(r)

# Numerical value, do not change to varname# else change theta to varnameans := [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 varnameans := [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 definedlocal 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 answerans := 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 definedlocal i, robot, ans;

# Defines the robotrobot := [];

# The links must be of type linkfor i from 1 to nargs do

if type(args[i], ’link’) thenrobot := [op(robot), args[i]];else error

"All robot links must be of type ’link’.";end if;

end do;

# All links collected in a listans := 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 definedlocal theta, d, a, alpha, Rotztheta, Transzd, Transxa, Rotxalpha,

A, ans;

# Picks the DH-parameters for the link objecttheta := link[1];d := link[2];a := link[3];alpha := link[4];

# Calculates the transformation matrix A

46

# Created the rotations/translationsRotztheta := 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/translationsA := Rotztheta.Transzd.Transxa.Rotxalpha;

# Returned answerans := 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 definedlocal 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) dolink[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 definedlocal dim, ans;

# Required dimension of matrix Tdim := 4,4;

if op(1, T) = dimthen# The position of the tip of the link relative to# coordinate frame ians := 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 definedlocal dim, ans;

# Required dimension of matrix Tdim := 4,4;

if op(1, T) = dimthen# The rotation matrix from the transformation matrix Tans := 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 pdim := 3;

if op(1, p) = dimthen

# The position of the tip of the link relative to# coordinate frame id := DHpositionT(T);

# The rotation matrix from the transformation matrix TR := DHrotationT(T);

# Transforms the vector p according to p_i = R*p + dans := 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-1zunit := Vector[column]([0, 0, 1]);

z[0] := zunit;

# Defines the unit matrix R0R[0] := Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]]);

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

# Calculates the transformation matrices T_i

50

T := DHmatrixT(robot);

# Help variablen := nops(robot);

# Creates the Jv-matrix of needed sizeJv := Matrix(3, n, 0);

# Calculates R_i, z_ifor i from 2 to n do

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

end do;

# Calculates d_ifor i from 1 to n do

d[i] := DHpositionT(T[i]);end do;

# Calculates the linear part of the Jacobianfor i from 1 to n do

# Test if revolute or prismatic jointif 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 R0R[0] := Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]]);

# Calculates the transformation matrices T_iT := DHmatrixT(robot);

# Help variablen := nops(robot);

# Creates the Jomega-matrix of needed sizeJomega := Matrix(3, n, 0);

# Calculates the angular part of the Jacobianfor i from 1 to n do

# Test if revolute or prismatic jointif 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 thenz[i-1] := zunit;elseR[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 ofthe robot

DHjacobian := proc(robot::robot)

local J, Jomega, Jv, ans;

# Linear part of the JacobianJv := DHlinearjacobian(robot);

# Angular part of the JacobianJomega := DHangularjacobian(robot);

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

ans := J;

end proc;

53

B Malpe implementation, another way to do it

B.1 A more direct approachIt may be interesting to compare the DH-toolbox in this report to the calculationof the kinematics of the Stanford manipulator, presented in [11], pages 435–441.In [11] has used the linalg package, whereas the LinearAlgebra package hasbeen used here in this report. The linalg package is older and therefore theprocedures used can be slightly different. Also, when treating the problem in adifferent way, other procedures may be used. Another difference is that the workin 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 withoutdoing 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 aremultiplied with evalm in order to get the transformation matrix T6 from whichthe position and orientation of the tip of the manipulator can be described.Thereafter the Jacobian is calculated. First the Ti-matrices is splitted into atranslational part and rotational part by means of subvector and submatrix.The linear velocity part of the Jacobian, described in Section 7.2, is calculatedwith jacobian. The angular part, see Section 7.3, is derived by first mappingthe diff procedure to all matrix coefficients, then use transpose, evalm andsimplify. 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 arethen placed below each other in the Jacobian matrix with stack. Then the rankof the linear part of the Jacobian is discussed. If this does not have maximumrank (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, detand addrow some singular states are found. [11]

54