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 Engineering Linköpings universitet
SE-581 83 Linköping, Sweden
WWW: http://www.control.isy.liu.se
AUTOMATIC CONTROL REGLERTEKNIK LINKÖPINGS UNIVERSITET
Technical reports from the Automatic Control group in Linköping are available from http://www.control.isy.liu.se/publications.
Abstract
This report studies robot modelling by means of the computer algebra tool Maple. First coordinate systems are described, and the more general way with transformation matrices is chosen in the further work. The position kinematics of the robot are then described by homogeneous transformations. The Denavit-Hartenberg representation is used, which is a systematic way to develop the forward kinematics for rigid robots. The velocity kinematics is then described by the Jacobian. The industrial robot IRB1400 from ABB Robotics is used as an example of the theory and to show the use of the procedures developed in the Maple programming language.
Keywords: Maple, industrial robot, homogeneous transformation, Denavit-Hartenberg representation, forward kinematics, Jabcobian
Avdelning, Institution Division, Department
Division of Automatic Control Department of Electrical Engineering
Datum Date 2007-08-09 Språk Language Svenska/Swedish Engelska/English Rapporttyp Report category Licentiatavhandling Examensarbete C-uppsats D-uppsats Övrig rapport
URL för elektronisk version
http://www.control.isy.liu.se
ISBN — ISRN
—
Serietitel och serienummer Title of series, numbering
ISSN 1400-3902
LiTH-ISY-R-2723
Titel Title
On robot modelling using Maple
Författare Author
Johanna Wallén
Sammanfattning Abstract
This report studies robot modelling by means of the computer algebra tool Maple. First coordinate systems are described, and the more general way with transformation matrices is chosen in the further work. The position kinematics of the robot are then described by homogeneous transformations. The Denavit-Hartenberg representation is used, which is a systematic way to develop the forward kinematics for rigid robots. The velocity kinematics is then described by the Jacobian. The industrial robot IRB1400 from ABB Robotics is used as an example of the theory and to show the use of the procedures developed in the Maple programming language.
Nyckelord Keywords
Contents
1 Introduction 4
2 Related research 4
2.1 The chosen models . . . 4
2.2 Why Maple? . . . 5
3 Basics about robots and coordinate systems 5 3.1 Common parts of the industrial robot . . . 5
3.2 Pairs and joints . . . 5
3.3 Coordinate systems. . . 6
4 Position kinematics 8 4.1 Translation . . . 9
4.2 Rotation . . . 9
4.3 Different representations of rotations . . . 10
4.4 Rigid motion . . . 11
4.5 Differentiation of the transformation matrix . . . 12
5 Denavit-Hartenberg representation 13 5.1 Homogeneous matrix . . . 13
5.2 Standard Denavit-Hartenberg representation . . . 14
6 D-H representation – a robot example 15 6.1 Link parameters and joint variables. . . 16
6.1.1 Configuration of IRB1400 . . . 17
6.1.2 Transformed joint variables . . . 17
6.1.3 Mathematical summary . . . 18
6.1.4 Thoughts about the closed kinematic chain . . . 18
6.1.5 How to do in Maple. . . 19 6.2 Transformation matrices . . . 23 6.2.1 How to do in Maple. . . 25 6.3 Rigid motion . . . 26 6.3.1 How to do in Maple. . . 27 7 Velocity kinematics 27 7.1 Manipulator Jacobian . . . 28 7.2 Linear velocity . . . 28 7.2.1 How to do in Maple. . . 30 7.3 Angular velocity . . . 31 7.3.1 How to do in Maple. . . 32 7.4 Resulting Jacobian . . . 33 7.4.1 How to do in Maple. . . 34
7.5 Discussion about the Jacobian . . . 34
7.5.1 Interpretation of the Jacobian. . . 35
7.5.2 Singularities. . . 35
9 Summary of example IRB1400 in Maple 37
9.1 Step by step. . . 37
10 Concluding remarks and future work 39 A Malpe implementation 41 A.1 DH.mpl. . . 41 A.2 type . . . 41 A.3 DHparameter . . . 43 A.4 DHlist2link . . . 43 A.5 DHvariablechange . . . 44 A.6 DHlink. . . 45 A.7 DHrobot . . . 45 A.8 DHmatrixA. . . 46 A.9 DHmatrixT. . . 47 A.10 DHpositionT . . . 48 A.11 DHrotationT . . . 49 A.12 DHtransformationT . . . 49 A.13 DHlinearjacobian . . . 50 A.14 DHangularjacobian . . . 51 A.15 DHjacobian . . . 52
B Malpe implementation, another way to do it 54 B.1 A more direct approach . . . 54
1
Introduction
In this report a first step towards making a toolbox for industrial robot modelling in the computer algebra tool Maple [13] is taken. The aim with this project is not to build a comprehensive and complex toolbox, but to study relatively simple robot models in order to understand the techniques.
The work leads towards a modelling platform for efficient derivation of the necessary equations that in the future can aid e.g., developing different model representations of robot structures with or without mechanical flexibilities or performing sensor fusion. Additional information from sensors demands signal processing and sensor fusion, which is hard to achieve without good models and good modelling tools. The idea is to provide a platform where it is simpler to evaluate different kinds of sensors and also various sensor locations using Maple as a tool to symbolically generate the kinematic models. Thereafter the models will be incorporated in Matlab- or C-code to implement, e.g., state estimation using an Extended Kalman Filter (EKF) algorithm.
The numerical calculations described in this report can also be made by means of Robotic Toolbox in Matlab [4], but the symbolic manipulations can-not be performed in Robotics Toolbox.
2
Related research
An example of previous work in this area is [3], where a Mathematica based approach to robot modelling is presented by Bienkowski and Kozlowski. The kinematic derivation in [3] does, however, not use the Denavit-Hartenberg (D-H) representation, which is a standard in the robotics field (see Section 5 for a description). A fairly well-known contribution is Robotics Toolbox [4], developed by Corke, where numerically calculations of kinematics, dynamics and trajectory planning are possible. It provides simulation and also analysis of results from real robot experiments. Another contribution by Corke is [6], but the focus there is on the dynamics and how dynamic models of robots can be simplified using Maple. See also [5] for the details. A Mathematica package, Robotica [15], has also been developed by Nethery ans Spong. This package is based upon [17] and gives support for both kinematic as well as dynamic modelling. Although similar to what in presented in this report, the package Robotica is no longer supported or updated and it is not sure that it works in the last version of Mathematica without editing the source code (according to the homepage of Mark W. Spong, one of the authors of Robotica).
2.1
The chosen models
The work in this report can be done for all axis and the model is without joint flexibilities. The examples in this report show the results for just the two or three first axis of the robot model IRB1400 [16] from ABB Robotics, since it is hard to take in the whole picture because of the size of the expressions for all six axis.
2.2
Why Maple?
There are many computer algebra tools like, e.g., Matlab Symbolic Toolbox, Mathematica and Maple. They all have different benefits and disadvantages. The reason why Maple is chosen in this case is that ABB uses this tool and it is a large benefit to be able to deeper understand the work done at ABB.
The work in this report is performed in Maple 10 and 11 and in this report the implemented toolbox is called DH-toolbox.
3
Basics about robots and coordinate systems
It is needed to define suitable coordinate systems for the following work, but first of all it is necessary to explain common terms for the different parts used in the industrial robot modelling.
3.1
Common parts of the industrial robot
The individual bodies that together form a robot are called members or links. Generally a robot with n degrees-of-freedom has n + 1 links and the base of the robot is defined as link 0. It also has n joints. Most of the industrial robots have six degrees of freedom, which makes it possible to control both the position of the tool and its orientation. A systematic way to calculate the number of degrees of freedom of a robot by means of the so-called Grübler or Kutzbach criterion is shown in [18].
The dynamics of the robot is coupled and therefore movements of one joint will also affect the other joints. For example link 2 attaches joint 2 to joint 3, i.e., a joint provides some physical constraints on the relative motion between the two links. Coordinate system number 3 is placed in the end of link 3, i.e., in joint 3, so the coordinates of each point of link 3 are constant in coordiante frame 3 for every movement of the robot links. See for example Figure 3-1, page 64, in [17] for an illustration of these notions.
The assumption of all links as rigid bodies makes the study of robots much easier to understand. For high-speed robots, there are significant elastic effects, and these should be taken into consideration. Non-rigid bodies, e.g., chains, cables or belts, may also be seen as links, because they momentarily act as rigid bodies. Also two links that are connected to each other in such a way that no relative motion can occur between them can, from a kinematic point of wiev, be considered as a single link. [18]
3.2
Pairs and joints
The ith joint has a variable, qi, that represents the relative displacement between
adjacent links, i.e., each joint has one degree of freedom. The total degrees of freedom for the robot is however equal to the degrees of freedom associated with the moving links minus the number of contraints imposed by the joints. [18]
The kind of relative motion between the links, connected by the joint, is determined by the contact surfaces (called pair elements) between the links. Two pair elements form a kinematic pair, and if the two links are in contact with each other by a substantial surface area it is called a lower pair. Otherwise, if the links are in contact along a line or at a point, it is called a higher pair. Examples
of lower pairs are revolute joint (also called turning pair), prismatic joint (called sliding pair), cylindrical joint, helical joint (called screw pair), spherical joint and plane pair. The frequently used universal joint is a combination of two intersecting revolute joints. Higher pairs are gear pair and cam pair. More about these joints and pairs and their corresponding number of degrees of freedom can be found in [18]. In this report it is sufficient to know that a revolute joint has a cylindrical shape where the motion is a rotation with angle θ. On the other hand, the prismatic joint can be described by a cube with side d and its motion is a translation along the defined z-axis.
The forward kinematics determines the pose, i.e., position and orientation, of the robot tool when the joint variables qiof the robot are given. For a revolute
or rotational joint, the variable qi is the angle of rotation between the links. In
the case of prismatic or sliding joint, the variable is the link extension. This is described in more detail in Section5.2.
The joint variables q1, . . . , qn form a set of generalised coordinates for an
n-link robot. Figure 1 shows the joint variables qi for IRB1400 from ABB
Robotics, which only has revolute joints. [4, 17]
q1 q2 q3 q4 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 revolute or prismatic joints (both having one degree of freedom), and further work is therefore only applied to revolute or prismatic joints.
3.3
Coordinate systems
The simplified robot model presented in Figure2 with two arms in the ground plane has no gravitational force. One way to represent the two axis is to have a coordinate system with the unit vectors ˆr1and ˆθ1for the first arm and the unit
ˆx ˆy ˆr1 ˆθ1 q1 ˆr2 ˆθ2 q2
Figure 2: Simplified robot model with two arms in the ground plane. The figure comes from [2] and shows the global coordinates ˆx and ˆy and the local coordinate 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 unit vectors ˆr1 and ˆr2. The unit vectors ˆθ1 and ˆθ2 can for example be seen as a
rotation by an angle π2 from the ˆr1- and ˆr2-vectors. With this in mind it is
easier to understand and verify the derivatives of these unit vectors below
˙ˆr1= ˙q1θˆ1 (5) ˙ˆ θ1= − ˙q1rˆ1 (6) ˙ˆr2= ( ˙q1+ ˙q2)ˆθ2 (7) ˙ˆ θ2= −( ˙q1+ ˙q2)ˆr2 (8)
When ˆr1is differentiated it is only an angular change, because the length of
the 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 when differentiating ˆθ1, now with the outlook that ˆθ1 is rotated by an angle π2 from
ˆ
r1. It can now easily be seen thatθ˙ˆ1is in the −ˆr1-direction with length ˙q1from
Then the derivatives of the unit vectors for the second axis are derived. ˙ˆr2
is 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 length
from the inner derivative.
The second derivates of the unit vectors are derived in [2] and can be depicted as follows ¨ ˆ r1= ¨q1θˆ1− ˙q12ˆr1 (9) ¨ ˆ θ1= −¨q1rˆ1− ˙q21θˆ1 (10) ¨ ˆ r2= −( ˙q1+ ˙q2)2rˆ2+ (¨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 in
the ˆθ1-direction because only ˙q1is differentiated and ˆθ1is constant. The second
term with length ˙q21 from the inner derivative of ˆθ1 is in the ˆr1-direction as
expected, because it is a derivative of ˆθ1. The other derivatives can be verified
intuitively following a similar discussion.
In this section only the first two axes are described by these coordinate systems, but it is straight forward to move on and describe the other axes in a similar manner. The trick is to see that all necessary information about the orientation of the robot arm is collected in (1) as described above, because (2) is just (1) rotated by the angle π2. The derivatives were however more difficult to perform in Maple than expected. It is also more specific than the general homogeneous transformations, where the rotations and transformations between the coordinate systems are represented by a single matrix. These homogenous transformations are therefore used in the further work.
4
Position kinematics
Kinematics describes the movements of bodies without considerations of the cause. Position, velocity, acceleration and higher derivatives of the position variables are studied. Robot kinematics especially studies how various links move with respect to each other and with time [4]. Dynamics on the other hand describes the movements with respect to also the forces that caused the motion. There are two ways to describe the position kinematics. Forward kinematics is one way where the idea of kinematic chains is used and the position and orientation of the robot tool are determined in terms of the joint variables. Kinematic chains means that the robot is seen as a set of rigid links connected together by different joints. In this report the forward kinematics is studied, and the work is performed with homogeneous transformations (see Section4.4) and the Denavit-Hartenberg (D-H) representation (see Section 5). The other way is inverse kinematics which means to calculate the joint configuration of the robot from the position and orientation of the robot tool. This is in general a much more difficult problem to solve than forward kinematics. [16]
4.1
Translation
First a parallel translation of the vector p1by the vector d10in coordinate frame 0
is described. It can shortly be written as
p0= p1+ d10 (13)
4.2
Rotation
The transformation between coordinate frame 0 and frame 1 is described and then frame 2 is added to show the structure of the combined transformations even more.
The basic rotation matrix R0
1 describes the transformation of the vector p
between frame 0 and frame 1 as
p0= R10p1 (14)
The matrix R1
0 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. R10 can thus be given by
R10= x1· x0 y1· x0 z1· x0 x1· y0 y1· y0 z1· y0 x1· z0 y1· z0 z1· z0 (15)
For example a rotation with an angle θ around the z-axis in the base frame can be 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 inverse transformation p1= R01p0 is given by
R01= (R10)−1 = (R01)T (17) Now coordinate frame 2 is added, which is related to the previous frame 0 and frame 1 as follows.
p0= R10p1 (18)
p0= R20p2 (19)
p1= R21p2 (20)
Combining the rotation matrices gives the transformation from frame 2 to frame 0 according to
p0= R10R21p2= R02p2 (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 followed
by rotating the frame 2 relative to frame 1 according to R2
transformation matrices cannot be changed, because then the interpretation and the result of these rotations are quite different. Rotation is unlike trans-lation a matrix quantity and therefore the rotational transformations do not commute. [17]
Above the rotations are made around successive different frames, but some-times it is more desirable to rotate around the fixed frame 0 all the time. This is performed by multiplying the transformation matrices in the reverse order compared to (21), giving the rotation matrix
R20= R21R10 (22)
4.3
Different representations of rotations
The elements in the rotation matrix R are not independent. A rigid body can at most have three rotational degrees of freedom, and thus at most three in-dependent variables are needed to specify the orientation. There are different representations, each of them describing an arbitrary rotation by three indepen-dent quatities, except the quaternion representation which uses four quantities. Axis/angle The axis/angle representation is based on the idea that a rotation matrix always can be represented by a single rotation about a single axis in space by a suitable angle. It can be described by the relation
R = Rk,θ (23)
where k is the unit vector that defines the axis of rotation and θ is the angle of rotation about the axis. The equation is therefore called the axis/angle representation of R. The representation is however not unique, since a rotation by the angle −θ about −k gives the same result.
Euler angles The method Euler angles is a more common way to specify the rotation matrix R. The Euler angles θ, φ and Ψ are obtained by the following successive rotations. For example the orientation of frame 1 relative to frame 0 are specified by first rotating about the z0-axis by the angle φ. Then rotate
about the current y-axis by the angle θ and last but not least rotate about the current z-axis (which now is the z1-axis) by the angle Ψ. The resulting
rotational transformation R10 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 product of successive rotations about the principal coordinate axes x0, y0 and z0 in a
specified order. The roll ψ, pitch θ and yaw Ψ angles can be seen in Figure 2-8 in [17]. First yaw about x0 by the angle Ψ, then pitch about y0 by the angle θ
and last roll about z0 by the angle φ. This is described by the matrix product
Quaternions Quaternions are an extension to complex numbers and a quater-nion consists of four elements q1. . . q4. It can mathematically be written
q = q1+ iq2+ jq3+ kq4 (26)
where i, j and k are mutually orthogonal imaginary units with special properties. A quaternion can also be described as a tuple, but that would lead too far in this very short description of quaternions.
They can be used to compactly describe an arbitrary rotation. Two vectors p0 and p1 are separated by the angle θ, i.e., there is a homogeneous
transfor-mation
p0= Rotk,θ (27)
The rotation is performed around the axis defined by the unit vector k. By means of quaternions, this expression can be rewritten as
q = cos(θ/2) k sin(θ/2)
!
(28)
p0= q ∗ p1∗ ¯q (29)
See for example [9], where both a convention corresponding with the column vector and the row vector convention of homogeneous transformation matrices are presented. In [10] the quaternions used in transformation matrices are de-scribed and an example of a quaternion based solution to the inverse kinematics problem for the robot arm PUMA 560 is shown.
4.4
Rigid motion
The most general move between the coordinate frames 0 and n can be described by a pure rotation combined with a pure translation. This combination is called a rigid motion if the rotation matrix Rn
0 in the equation
p0= Rn0pn+ dn0 (30)
is orthogonal, i.e., (Rn0)TRn0 = I. Important properties of the rotation matrix R worth knowing are det(R) = 1 and the set of all 3×3 rotational matrices are collected by in the Special Orthogonal group of order 3, called SO(3). [17]
According to [17] the rigid motion can be represented by a matrix H of the form
H = R d
0 1 !
(31) Since R is orthogonal, the inverse transformation H−1 is defined by
H−1= R
T −RTd
0 1
!
(32)
These transformations are called homogeneous transformations. To be able to write the transformation as a matrix multiplication, the homogeneous
represen-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= H01P1 (35)
Combining two homogeneous transformations, e.g., p0 = R10p1 + d10 and
p1= R21p2+ d21, gives H01H12= R 1 0 d10 0 1 ! R21 d21 0 1 ! = R 1 0R21 R10d21+ d10 0 1 ! (36)
4.5
Differentiation of the transformation matrix
A skew symmetric matrix S is defined as
ST + S = 0 (37)
If the rotation matrix R(θ) is dependent of the variable θ, the derivative can be described by means of the skew symmetric matrix S
dR
dθ = SR(θ) (38)
In this report the rotation matrix R(q) depends on the joint variables q. In the same way, a time varying matrix R(t) is described as
˙
R(t) = S(t)R(t) (39)
with the skew symmetric rotation matrix S(t). Since S(t) is skew symmetric, it can also be represented as S ω(t) for the unique vector ω(t), which is the an-gular velocity of the rotating frame with respect to the fixed frame, for example ωn
0 for the tool expressed in the base frame 0. It then gives
˙
R0n(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 transformations between coordinate frames. How to use the properties of the skew symmet-ric matsymmet-rices in order to simplify the differentiation of the Jacobian is further described in Section8. [17]
5
Denavit-Hartenberg representation
The Denavit-Hartenberg representation, also called the D-H convention, de-scribes a systematic way to develop the forward kinematics for rigid robots. The classical method was presented by Jacques Denavit and Richard S. Harten-berg in 1955, see [8]. This convention to develop reference frames is commonly used in robotic applications.
There are two slightly different approaches to the convention, the so called standard D-H notation described in for example [17] and the modified D-H form found in [7]. A comparison between the forms can be found in Figure 8 in [4], where it can be seen that frame i − 1 in the standard form are denoted frame i in the modified form. The expressions for the link transformation matrices are different, but both notations represent a joint as two translations and two rotations.
5.1
Homogeneous matrix
The homogeneous matrix Ai= Ai(qi) is a function of a single joint variable qi.
It describes the transformation of the coordinates from frame i to frame i − 1 under the assumption that the joints are either revolute or prismatic. The transformation matrix Tij that transforms the coordinates of a point expressed in frame j to frame i can then be written as
Tij = Ai+1Ai+2· · · Aj−1Aj, i < j (43)
The homogeneous transform Ai is of the form below, compare to (31)
Ai= Ri i−1 dii−1 0 1 ! (44)
The total homogeneous transform Tij is then given by
Tij= Ai+1· · · Aj = Rij dji 0 1 ! , i < j (45) where Rji = Ri+1i · · · Rj−1j , i < j (46) and dj−1i is given recursively by
dji = dj−1i + Rj−1i dj−1j , i < j (47) With i = 0 and j = n, (45) gives the homogeneous matrix Tn
0 which transforms
the coordinates from frame n to frame 0 T0n = R n 0 dn0 0 1 ! (48)
The position and orientation of the robot tool in the inertial coordinate frame 0 are thereby described by
which uses the current joint variable qi in every transformation step on the way
from frame n to frame 0. This results in the transformation of the vector pn
expressed in frame n to frame 0 as in the relation
pn0 = R0npn+ dn0 (50)
5.2
Standard Denavit-Hartenberg representation
Each homogeneous transformation Ai is in the D-H representation regarded as
a product of four basic transformations
Ai= Rotz,θiTransz,diTransx,aiRotx,αi (51) The D-H link parameters θi, ai, di and αi are defined as follows. They are all
parameters of link i and joint i.
• Angle θi: angle between xi−1- and xi-axis measured in the plane normal
to the zi−1-axis.
• Length ai: distance from the origin oi(frame i) to the intersection between
xi- and zi−1-axis measured along the xi-axis.
• Offset di: distance between the origin oi−1(frame i − 1) and the
intersec-tion of xi-axis with zi−1-axis measured along the xi-axis.
• Twist αi: angle between zi−1- and zi-axis measured in the plane normal
to xi. [17]
When performing the translations di, ai and the rotations θi, αi in (51) it
is important that they are applied in right order. First a rotation θi around
the z-axis is performed, then a translation di along the new z-axis followed by a
translation ai along the new x-axis and last but not least a rotation αi around
the new x-axis. See for example Figure 3-4, page 69, in [17] for an illustration of the transformations.
Under the circumstances that the origin oi (frame 0) do not need to lie at
joint i, the coordinate frames 0, . . . , n for the robot can always be chosen in such a way that the following two conditions are satisfied [17]
(D-H1) xi is perpendicular to zi−1
(D-H2) xi intersects zi−1
(52)
With the four basic transformations, the homogeneous transformation Ai
then 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 θicos αi sin θisin αi aicos θi
sin θi cos θicos αi − cos θisin αi aisin θ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=
Ri
i−1 dii−1
0 1
!
It can directly be seen that
Rii−1=
cos θi − sin θicos αi sin θisin αi
sin θi cos θicos αi − cos θisin αi
0 sin αi cos αi (54) dii−1= aicos θi aisin θi di (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 a
prismatic joint, see also Section5.1for a discussion about different types of links and joint variables. [17]
One important thing to notice is however that choices of the coordinate frames are not unique. [17]
6
D-H representation – a robot example
This section describes the derivation of joint variables and transformation ma-trices for the robot IRB1400 from ABB Robotics shown in Figure3. Information
regarding IRB1400 is mainly taken from [16]. In this section it is also described how these variables and matrices are treated in the DH-toolbox implemented in the Maple programming language.
Figure 3: The industrial robot IRB1400 with a payload of 5 kg and a maximum range of 1.44 m. Picture from ABB Robotics public archive [1].
6.1
Link parameters and joint variables
The D-H link parameters for IRB1400 are given in Table 1 and the robot is positioned in the configuration defined by these parameters.
Table 1: The D-H link parameters for the robot IRB1400 [16].
Joint/Link αi ai θi di i [rad] [m] [rad] [m] 1 −π/2 0.15 0 0.475 2 0 0.6 −π/2 0 3 −π/2 0.12 0 0 4 π/2 0 0 0.72 5 −π/2 0 0 0 6 0 0 π 0.085
What are the joint variables in this example with IRB1400? They have to be known before the velocity kinematics of the robot can be derived. Let us say that the joint variables for this robot is, generally speaking, the variables φi
according to
φ = φ1 φ2 φ3 φ4 φ5φ6
T
(56) These joint variables can be determined by reasoning around the movement of the actual robot link and then verified by a sketch of the coordinate frames for the links in order to see that the variables gives the expected appearance and
structure of the robot. In this report we are satisfied with the reasoning, and the sketch is left for prospective readers.
The movement of each link is compared to the coordinate frame for the link. Since it is always a rotation around the z-axis in the local coordinate frame for each link of the robot IRB1400, this gives that all joints are revolute and the joint variable φi is equal to the joint angle θi for each joint, see Section5.2for
a description of the D-H representation and the variables involved.
But, are these variables the “real” D-H joint variables for the robot? Or do we need to modify them because of the configuration of the robot IRB1400? 6.1.1 Configuration of IRB1400
Loosely speaking, there are two types of industrial robots — those with an open kinematic chain and those who have a closed kinematic chain. Kinematic chains is described in Section 4 and the robot is now seen as a set of rigid links connected together by different joints, under the assumption that each joint only has a single degree of freedom. In Section 3.2we discussed different types of joints and established that all types of joints can be described by only revolute or prismatic joints with one degree of freedom each, which satisfies this assumption.
The chain of robot links can be open, with every link connected to every other link by one and only one path. When the kinematic chain is connected to every other link by at least two distinct paths, it forms two or more closed-loop chains and therefore is a closed kinematic chain. [18]
The robot IRB1400 has, as can be seen in Figure1or Figure3, a mechanical coupling between motor 3, placed in the foot of the robot, to the actual link 3. We have a closed kinematic chain, and the structure in this particular case is also called a parallelogram linkage structure to link 3 [17]. In practice this parallelogram linkage structure means that joint 2 and joint 3 are coupled to each other. If motor 2 is moved, but not motor 3, the angle between the upper arm (x3-axis) and zo-axis is constant, so that link 3 keeps the same orientation
with respect to the base frame, i.e., when moving motor 2 it will affect both the variables θ2 and θ3. The profitable idea with this parallelogram linkage
structure is that motor 3, which moves axis 3, is placed below this axis. The upper robot links can then be made thinner and stiffer and it is easier and cheaper to move the robot links. It also reduces the production cost since less material is needed. A drawback with this structure is however that the movement of the robot is more restricted and the robot cannot rotate the upper links with an angle π (called bending backwards) — instead the whole robot has to rotate around. [16]
6.1.2 Transformed joint variables
The D-H convention is based on the assumption that a movement of a joint does not affect any other joint, i.e., we have an open kinematic chain. As described, IRB1400 has a closed kinematic chain, but in this case it is of a bilinear form, which gives the opportunity to transform its joint variables to the corresponding D-H joint variables for the transformed, open structure.
The need of transforming the joint variables to “real” D-H variables (also called D-H parameters in [17]) can also be seen from the fact that when all
variables in the D-H representation are zero, the robot is placed in a certain position and orientation (pose). (In this case it by the way happens to be a position that the robot never can reach.) This “zero variable pose” is however different from the pose of the robot when its joint angles are zero, so we need to transform the variables in order to achieve an agreement between the zero robot pose and “zero variable pose”.
6.1.3 Mathematical summary
As a summary, in order to get the D-H parameters θi for the robot, the joint
angles φi need to be translated from its origin with the vector θ0 and
trans-formed by the matrix θT rans. Generally this gives the expression for the D-H
parameters θ [16] as follows
θ = θT ransφ + θ0 (57)
This can be performed in Maple by DHparameter, see also Section A.3. For the IRB1400 case with mechanical coupling between joint 2 and joint 3 and the zero pose given in Table1, it results in
θT rans= 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 −π/2 0 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 as general variables used and differentiated in for example Section7and Section8 and separate them from the specific joint angles for each joint.
The D-H joint variables qi are in the IRB1400 case the joint angles θi
be-cause there are only rotations and no translations of the joints, as mentioned in Section 6.1. And now, after the transformation of the D-H joint variables, the robot stands in its zero pose when the joint angles of the robot are zero. The D-H link parameters αi, ai, di and joint variables qi= θi now looks like in
Table2.
6.1.4 Thoughts about the closed kinematic chain
In this report we have investigated certain types of closed kinematic chains that can be described on a bilinear form. The parameter transformation in (57) is performed at the beginning, i.e., when determining the D-H link parameters and joint variables. Another approach may be to calculate the kinematic re-lations using the joint variables for the actual robot and afterwards transform the result into the “real D-H” joint variables for the corresponding, transformed open kinematic chain.
A reasonable idea in the approach with the transformation afterwards is that with only linear cross-connections (57) it gives just an additional matrix
Table 2: The D-H link parameters αi, ai, di and joint variables qi= θi for the robot IRB1400 [16]. Joint/Link αi ai qi= θi di i [rad] [m] [rad] [m] 1 −π/2 0.15 θ1 0.475 2 0 0.6 θ2 0 3 −π/2 0.12 θ3 0 4 π/2 0 θ4 0.72 5 −π/2 0 θ5 0 6 0 0 θ6 0.085
multiplication when calculating, e.g., the Jacobian. The offset reasonably does not influence the Jacobian directly because the Jacobian contains derivatives, and the offset does not contribute in these derivatives.
In Robotics Toolbox for Matlab [4] the parameter transformation has been treated by an offset in the robot object. The offset is specified by a vector, which is added to the joint angles within the link object. It is in other words added before any kinematic or dynamic functions are operating on the joint angles. The offset vector is in a similar way subtracted when needed, e.g., after deriving the inverse kinematics. In [4] it is also described why there is a need for an offset; when the joint angles are zero the robot may be in an unachievable pose. The offset then makes it possible to perform the pose of the robot corresponding to zero joint angles.
6.1.5 How to do in Maple
Before being able to create robot links in Maple and bring them together to a robot, we have to know something about the different types in Maple. The input when creating a link is specified by a sequence of all allowed types of inputs. This facilitates a lot if one know how Maple thinks about types and operands. Therefore this section begins with a try to summarize the most important features of types and operands in Maple, followed by a description of how a link and a robot is created in the DH-toolbox.
Operands in Maple θ(t) and θ1(t) are distinguished by the op command.
As is shown below the symbol θ is different parts in the two expressions. The first operand in θ(t) is θ, but in θ1(t), we have to investigate the first operand of
the expression, i.e., θ1 (the variable t is the second operand in the expression)
and then take the first operand of this subexpression to get θ. How to do in Maple to pick out the symbol θ is shown below.
op(0, theta(t))
op(0, op(0, theta[1](t)))
whattype(theta(t) - 1); whattype(theta(t) - pi);
gives the same answer, type ’+’, and one might think that they are both a sum of a function(symbol) (θ) and numeric (−1) or symbol (−π). Instead, −π is, seems very logical when one knows it, (−1)∗π, which is a multiplication, type ’*’ with operands of type integer (or numeric, larger type class) and symbol. −π2 is in the same way a multiplication (−12)∗π with operands of types fraction (or numeric, which is a larger type class than fraction) and symbol. And while θ(t) is of type function or the more specific type function(symbol), −θ(t) is of type ’&*’(numeric, function(symbol)). The structured types (types combined of defined Maple types) that are interesting as arguments for the links are collected in Table 3 and are discussed further on when creating the links in Maple.
Table 3: Possible arguments to the links. There are other, more general types, but the types here are chosen because they seem to be as specific as possible (or as general as possible, if such a property is useful).
argument type in Maple
5 numeric, (also integer) 5.0 numeric, (also float)
π symbol π 2 ’&*’(numeric, symbol) θ symbol θ1 name(indexed) θ(t) function(symbol) θ1(t) function(symbol) θ(t) + 5 ’&+’(function(symbol), numeric) θ(t) + π ’&+’(function(symbol), symbol) θ(t) + π
2 ’&+’(function(symbol), ’&*’(numeric, symbol))
θ(t) − 5 ’&+’(function(symbol), numeric)) θ(t) − 5.0 ’&+’(function(symbol), numeric))
θ(t) − π ’&+’(function(symbol), ’&*’(numeric, symbol)) θ(t) − π2 ’&+’(function(symbol), ’&*’(numeric, symbol)) θ1− θ2 ’&+’(name(indexed), ’&*’(numeric, name(indexed)))
θ1(t) −
θ2(t)
’&+’(function(symbol), ’&*’(numeric, function(symbol))) −θ(t) − 5 ’&+’(’&*’(numeric, function(symbol)), numeric)
−θ1(t) + 0 ’&*’(numeric, function(symbol))
type(-pi, ’&*’(numeric, symbol)); type(-pi, (numeric &* symbol);
Now one example that can be a bit confusing when the reason behind it is not known.
type(1+a, ’+’); with(VectorCalculus); type(1+a, ’+’);
It first shows true, but, after using the package VectorCalculus (which has a “vector +”), the ordinary type ’+’ is changed to the plus operator in the package VectorCalculus. So, with type(1+a, :-’+’) instead, the global, ordinary + without any extra packages loaded, is used. One valuable comment regarding :-’+’ is that this is an operand that invokes the operator ’+’ in the original, global package which is not extended with other packages. Generally a special operator in a certain package is invoked by package:-operator. A simple example is LinearAlgebra:-Transpose(MyMatrix).
During the tests with types in Maple, we have found out the following structure of types. The type numeric is a larger type class, containing the subclasses integer, fraction and float.
D-H parameters First, the D-H link parameters αi, ai, di, joint variables
θi and the translation parameter vector θ0 for the robot IRB1400 as defined
in (58) are derived in Maple as follows.
DHalpha := Vector[column]([-Pi/2, 0, -Pi/2, Pi/2, -Pi/2, 0]); DHa := Vector[column]([ 0.15, 0.6, 0.12, 0, 0, 0 ]);
DHtheta := Vector[column]([ theta[1](t), theta[2](t), theta[3](t), theta[4](t), theta[5](t), theta[6](t) ]); DHd := Vector[column]([ 0.475, 0, 0, 0.72, 0, 0.085 ]); DHtheta0 := Vector[column]([ 0, -Pi/2, 0, 0, 0, Pi ]); The matrix θT ransin (57) must also be defined in Maple.
theta||Trans := Matrix([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, -1, 1, 0, 0, 0], [0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]]);
Now the transformed D-H parameters θ in (57) can be calculated by means of the function DHparameter, see SectionA.3.
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 i is therefore here formed as a list with the structureθi di ai αi jointtype as is shown below for a robot example where the first link is prismatic and the second link is revolute.
link1 := DHlist2link(theta[1], d[1], a[1], alpha[1], p); link2 := DHlist2link(theta[2], d[2], a[2], alpha[2], r); which gives the answers
link1 :=hθ1 d1 a1 α1 p
i link2 :=hθ2 d2 a2 α2 r
i
of type link, defined in DH-toolbox. The link type is a list of allowed types, see SectionA.2for the definition and SectionA.4for how to use the procedure DHlist2link to create a link of type link from the specified link parameters. Possible types of arguments which give type link are collected in Table 3. Structured types in Maple is also discussed earlier in this section and can be seen as a supplement to the table. As is clearly shown, there are lots of different types in Maple that have to collaborate with us...
The expressions in Table 3can be combined to one structured type, which consists of the following types listed below in order to handle mixed orders of the operands.
• numeric, • symbol,
• name(indexed), • function(symbol),
• ’&*’(numeric, {symbol, function(symbol)}),
• ’&+’({numeric, symbol, name(indexed), function(symbol), ’&*’(numeric, {symbol, name(indexed), function(symbol)})}, {numeric, symbol, name(indexed), function(symbol),
With DHvariablechange the actual D-H variable can be changed to a chosen variable name according to following example. In Section6.1.3the variable name q is used and we use it in the sequel too for convenience.
link1 := DHlist2link(theta[1], d[1], a[1], alpha[1], p); link2 := DHlist2link(theta[2], d[2], a[2], alpha[2], r); DHlink1 := DHvariablechange(link1, q);
DHlink2 := DHvariablechange(link2, q); gives the answers
DHlink1 :=hθ1 q1 a1 α1 pi DHlink2 :=hq2 d2 a2 α2 ri
of type link, where the D-H joint variable θi is exchanged in the case of a
revolute link and the D-H joint variable di in the case of a prismatic link.
When the links are numerical, there is of course no exchange to the chosen D-H variable. See also SectionA.5for the details. The creation of a link and change of variable name can also be done directly by using DHlink, see SectionA.6and the example below.
DHlink1 := DHlink(theta[1], d[1], a[1], alpha[1], p, q); gives as above DHlink1 :=hθ1 q1 a1 α1 piof 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 givesh−q2(t) + q3(t) d3 a3 α3 riandh−θ2(t) + θ3(t) q3 a3 α3 pi.
A robot in Maple A robot model is constructed by placing a number of robot links with proper characteristics (i.e., of type link) after each other in a list. The procedure DHrobot takes at least one link as argument, and can also take arbitrarily many links. For example, with link1 and link2 defined as above a robot can be defined as
robot := DHrobot(link1, link2);
which gives the output robot :=h hθ1 q1 a1 α1 pi,hq2 d2 a2 α2 ri i of type robot, see the definition in Section A.2. DHrobot can be seen in Sec-tionA.7.
6.2
Transformation matrices
First, as an example, the homogeneous transformations for the first two links in the IRB1400 example according to (53) are examined when the joint variables φi (57) are zero, i.e., the zero pose of the robot and D-H joint variables qi= θi
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 links and the way to go between them. T2
0 then gives the position d20and orientation
R2
0 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
T02= 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)
T03 gives the same for frame 3 expressed in the base coordinates (frame 0)
T03= 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 base frame 0 is T06= A1A2A3A4A5A6≈ 0 0 1 0.955 0 1 0 0 −1 0 0 1.195 0 0 0 1 (66)
From this it can be seen that the last column represents the position of the robot 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 the robot is 0.995 m in the x0-direction, 0 m in the y0-direction and 1.195 m in the
z0-direction. Compared to the blueprint of the actual robot IRB1400 in [16],
this gives the same result. 6.2.1 How to do in Maple
Homogeneous matrices The homogeneous matrices are described in Sec-tion 3.2 and are in the DH-toolbox calcluated by means of the procedure DHmatrixA, see Section A.8. For example, a revolute link with the D-H joint variable θ1 exchanged to the variable name q1
link1 := DHlink(theta[1], d[1], a[1], alpha[1], r, q); A[1] := DHmatrixA(link1);
gives the homogeneous matrix (53)
A1=
cos q1 − sin q1cos α1 sin q1sin α1 a1cos q1
sin q1 cos q1cos α1 − cos q1sin α1 a1sin q1
0 sin α1 cos α1 d1 0 0 0 1 (68)
With all D-H parameters constant according to Table1, the first homogeneous matrix A1 is given by
link1 := [-Pi/2, 0.15, 0, 0.475, r]; A[1] := DHmatrixA(link1);
which gives the same result as in (60). If, as an illustrative example of an interesting feature in Maple, all link parameters are of type integer
link1 := [1, 2, 3, 4, r]; A[1] := DHmatrixA(link1); the result becomes
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 numerically using floating-point arithmetic.
Transformation matrices Now the general transformation matrices Ti 0(45)
are derived in Maple. The transformation matrices below are given with DHmatrixT, see Section A.9. In this example the robot consists of the first two links and is created with the commands
link1 := [theta[1], 0.475, 0.15, -Pi/2, r]; link2 := [theta[2] - Pi/2, 0, 0.6, 0, r]; myrobot := DHrobot(link1, link2);
T := DHmatrixT(myrobot); and the result is of type robot
myrobot :=h hθ1(t) 0.475 0.15 −π 2 r i ,hθ2(t) −π 2 0 0.6 0 r i i and the transformation matrix T, which is a table of the transformation matrices T1and T2. The respective matrices can be viewed by typing the first and second
element in the table, i.e., T[1] and T[2], which gives
T01= A1=
cos θ1 0 − sin θ1 0.15 cos θ1
sin θ1 0 cos θ1 0.15 sin θ1
0 −1 0 0.475 0 0 0 1 (70) T02= A1A2 =
cos θ1 0 − sin θ1 0.15 cos θ1
sin θ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 θ1sin θ2 cos θ1cos θ2 − sin θ1 0.6 cos θ1sin θ2+ 0.15 cos θ1
sin θ1sin θ2 sin θ1cos θ2 cos θ1 0.6 sin θ1sin θ2+ 0.15 sin θ1
cos θ2 − sin θ2 0 0.475 + 0.6 cos θ2
0 0 0 1 (71) As can be seen, the size of the expressions are increasing, so the transforma-tion matrices for the robot with more than just the two first links will not be presented here (too large!).
6.3
Rigid motion
In the IRB1400 example, we have as previously, the D-H parameters given according to (57) with the parameter values in Table2. As an example of how the transformation matrices derived above can be used, we study the vector
p2= 1 1 1 (72)
expressed in coordinate frame 2. It is transformed to the base frame 0 by means of the transformation matrix
R20=
cos θ1sin θ2 cos θ1cos θ2 − sin θ1
sin θ1sin θ2 sin θ1cos θ2 cos θ1
cos θ2 − sin θ2 0 (73)
and the vector from the origin to coordinate frame 2
d20=
0.6 cos θ1sin θ2+ 0.15 cos θ1
0.6 sin θ1sin θ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= R20p2+ d20 (75) This gives p20=
1.6 cos θ1sin θ2+ cos θ1cos θ2− sin θ1+ 0.15 cos θ1
1.6 sin θ1sin θ2+ sin θ1cos θ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, DHrotationT and DHtransformationT, see SectionA.10, Section A.11and SectionA.12 and below how to use them. This example uses the first two links with the robot in the zero pose, as in Section6.3.
link1 := [theta[1], 0.475, 0.15, -Pi/2, r]; link2 := [theta[2] - Pi/2, 0, 0.6, 0, r]; myrobot := DHrobot(link1, link2);
T := DHmatrixT(myrobot);
p[2] := Vector[column]([1,1,1]); R02 := DHrotationT(T[2]);
d02 := DHpositionT(T[2]);
p[0] := DHtransformationT(p[2], T[2]); which gives the same result as in (76).
7
Velocity kinematics
The velocity kinematics relates the linear and angular velocities of a point on the robot to the joint velocities. The D-H joint variables represent the relative displacement between adjoining links. In this section the robot has n links. The industrial robot IRB1400 is used to illustrate the theory, and the joint variables are qi= θi in this case. If nothing else is mentioned, the reference used in this
7.1
Manipulator Jacobian
The forward kinematic equations derived in Section 5 determine the position and orientation of the robot tool given the D-H joint variables. The Jacobian of this function determines the linear and angular velocities of a point on the robot related to the joint velocities and it is one of the most important quantities in the analysis and control of the robot motion. It is used in many aspects in robot control, like planning and execution of smooth trajectories, determination of singular configurations, execution of coordinated motion, derivation of dynamic equations of motion and last but not least to transform the forces and torques from the tool to the joints. [17]
Generally the n-link industrial robot has the joint variables q = (q1 . . . qn)T.
The transformation matrix from the robot tool frame n to the base frame 0 in (45) is rewritten below for simplicity with D-H joint variables qi
T0n(q) =
Rn0(q) dn0(q)
0 1
!
where
dn0 = Ri−10 dni−1+ di−10 (77) When the coordinates already are expressed in the base frame, no transforma-tion to the base frame is needed and we therefore have T0
0(q) = I. This gives
according to the relation above that R0
0(q) = I and d00(q) = (0 0 0)T.
The angular velocity vector ωn
0 is defined as the skew symmetric matrix (see
also Section4.5)
S(ω0n) = ˙Rn0(R0n)T (78) and the linear velocity of the robot tool is
v0n= ˙dn0 (79)
where ˙dn
0 means the time derivative ddn
0
dt . Written on the form
v0n= Jvq˙ (80)
ω0n= Jωq˙ (81)
(80) and (81) can be combined to vn0 ωn0 ! = Jv Jω ! ˙ q = J0nq˙ (82)
J0nis called the Manipulator Jacobian, or just the Jacobian. It is a 6 × n-matrix
because it represents the instantaneous transformation between the n-vector of joint velocities and the 6-vector which describes the linear and angular velocities of the robot tool. In the following sections it is described how the matrices Jv
for the linear velocities and Jω for the angular velocities can be derived.
7.2
Linear velocity
The linear velocity of the robot tool is described by just ˙dn0, see (79). Using the
chain rule of differentiation the result is ˙ dn0 = n X i=1 ∂dn 0 ∂qi ˙ qi (83)
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 are zero, which is achieved if the ith joint is actuated at unit velocity and all the other joints are fixed. There are two cases; a revolute joint and a prismatic joint and the respective joint is described by the relation (77).
Prismatic joint Study (77), repeated here dn0 = Ri−10 dni−1+ di−10
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 θicos αi sin θisin αi aicos θi
sin θi cos θicos αi − cos θisin αi aisin θ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, and
there are zeros in element A3,1, A4,1, A4,2 and A4,3. When two matrices of this
structure are multiplied, the di-variables are all in the fourth column, which
does not affect the behaviour of the R matrices, which is just the upper left 3 × 3-matrix.
Between the coordinate frames i and i − 1 we have the relation
dii−1= Ri−1i aix + diz (84)
which is a transformation from coordinate frame i to frame i − 1, which can be described on the form (30), i.e., pi−1 = Rii−1pi+ dii−1. In this case (84) the
translated and rotated vector results in di
i−1, with the translation term diz (diis
the D-H variable called offset) which is the offset from the origin in expressed cordinate frame i − 1 and aix (ai is the D-H variable called lenght) which is
the term to be rotated. The unit vectors x = (1 0 0)T and z = (0 0 1)T are expressed in frame i − 1.
If all other joints but the ith one are fixed, we have the time derivatives of di equal to zero for all joints except the ith one, i.e., ˙dj = 0, j 6= i. The
differentiation of (77) then together with (84) gives ˙
dn0 = R0i−1d˙ii−1= ˙diRi−10 z = ˙dizi−1 (85)
where di is the D-H variable called offset and zi−1 is the resulting vector in
the base frame when the unit vector z = (0 0 1)T in coordinate frame i − 1 is
expressed in the base frame with the transformation matrix Ri−10 . Compared to (83), with qi= difor the prismatic joint, this gives
∂dn0 ∂qi = zi−1= R0i−1z = R i−1 0 0 0 1 (86)
Revolute joint The revolute joint i is described by the relation (77), repeated here for convenience
dn0 = Ri−10 dni−1+ di−10
If only the ith joint is actuated, all joint angles except θi are fixed (constant)
and di−10 and Ri−10 are constant [16]. Differentiation of (77) now gives ˙
dn0 = Ri−10 d˙ni−1 (87) Since the joint is revolute and therefore the motion of link i is a rotation qi= θi
about the axis zi−1, according to [16] it gives
˙
dni−1= ˙θiz × dni−1 (88)
with z = (0 0 1)T in coordinate frame i − 1 as usual. When combining these equations the result, with Ri−10 z = zi−1, is
˙
dn0 = Ri−10 θ˙iz × dni−1 = ˙θiRi−10 z × R i−1 0 d n i−1= ˙θizi−1× (dn0 − d i−1 0 ) (89)
The last equality can be explained by the verbal picture that the positon of the tool, expressed in the base frame is dn
o. This is also the position of the link
position next to the tool expressed in the base frame, plus the position from this second last link to the last tool position, i.e., dn0 = di−10 + dni−1. Thsi relation can also be seen in for example Figure 5-1 [17].
From (83), with qi= θi, it can be seen that
∂dn 0 ∂qi = zi−1× (dn0 − d i−1 0 ) (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 Jviis Jvi= ∂dn 0 ∂qi =
zi−1, prismatic joint
zi−1× (dn0− d i−1
0 ), revolute joint
(92)
z0 is of course the unit vector z = (0 0 1)T in the base frame and n is the
number of links in the robot, as mentioned before. 7.2.1 How to do in Maple
This section is a relatively independent continuation of the examples in Maple described in former sections. In the IRB1400 case all n = 6 joints are revolute, as mentioned in Section 6.1. For example calculating the second column Jv2
according to (92) gives the relation
Jv2= z1× (d60− d 1
0) (93)
where z1= R10with the unit vector z = (0 0 1)T expressed in frame i − 1. (Here
z is named zunit just in order to avoid problems in Maple with different sizes and indices of the z-variable in the workspace.) In Maple this calculation looks like
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 same way. The symbolic result is however several (5-6) pages of Maple output, because the transformation matrices Ti
0 (and thereby di0) grow with increasing
index i. It is desirable to avoid deriving these expressions by hand, and instead use a computer algebra tool, e.g., Maple.
In order to be able to show something from an overall picture, the D-H link parameters for the robot in zero pose, given in Table 1, is used. Using the procedure DHlinearjacobian, see SectionA.13, in order to calculated the linear part of the Jacobian gives
link1 := DHlink(0, 0.475, 0.15, -Pi/2, r); link2 := DHlink(-Pi/2, 0, 0.6, 0, r ); link3 := DHlink(0, 0, 0.12, -Pi/2, r); link4 := DHlink(0, 0.72, 0, Pi/2, r); link5 := DHlink(0, 0, 0, -Pi/2, r); link6 := DHlink(Pi, 0.085, 0, 0, r);
myrobot := DHrobot(link1, link2, link3, link4, link5, link6); Jv := DHlinearjacobian(myrobot);
This results in the linear part of the Jacobian as follows
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 velocity
Angular velocities can be added vectorially if they all are expressed in a common coordinate frame [17]. It is then possible to express the angular velocity of each link in the base frame and then sum them to the total angular velocity of the robot tool.
Prismatic joint If for example the ith joint is prismatic, there is a pure translation and therefore the angular velocity vector is zero according to
Revolute joint If the ith joint instead is revolute, the joint variable is qi= θi
and the z-axis is the axis of rotation. The angular velocity expressed in the frame i − 1 is
ωi−1i = ˙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 from each link gives the following angular velocity of the robot tool for the robot with n links [17] ωn0 = ρ1θ˙1z + ρ2θ˙2R10z + . . . + ρnθ˙nR0n−1z = n X i−1 ρiθ˙izi−1= n X i−1 ρiq˙izi−1 (97) where ρi= 1, revolute joint 0, prismatic joint (98)
The following equation describes how the unit vector z in coordinate frame i − 1 is expressed in the base frame by means of the transformation matrix Ri−10 as in the case of the linear velocity. The resulting vector in the base frame is then called zi−1 according to
zi−1= Ri−10 z (99)
Comparison between (81) and (97) now gives the lower angular half Jω of the
Jacobian 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 transformed into the base frame. z0 is also here the unit vector z in the base frame.
7.3.1 How to do in Maple
The robot IRB1400 has only revolute joints, as described in Section6.1. When we take more than two links into account in the industrial robot model, the expressions for the Jacobian become very large, because they are functions of the D-H joint variables qi and the large transformation matrices (43).
There-fore, just in order to see how the technique works, the first four links of the robot IRB1400 are examined in Maple. The D-H variables are here manually transformed to “real” D-H variables, this can of course also be done with the procedure DHparameter, as described in Section6. This gives the first four links and the corresponding robot model as
link1 := DHlink(q[1](t), 0.475, 0.15, -Pi/2, r); link2 := DHlink(q[2](t)-Pi/2, 0, 0.6, 0, r );
link3 := DHlink(-q[2](t)+q[3](t), 0, 0.12, -Pi/2, r); link4 := DHlink(q[4](t), 0.72, 0, Pi/2, r);
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 in order to avoid problems with the variable names in the Maple workspace.
T := DHmatrixT(myrobot);
zunit := Vector[column]([0, 0, 1]); R[1] := DHrotationT(T[1]);
z[1] := R[1].zunit;
The part of the angular Jacobian for the second joint, Jω2, then becomes
Jω2= − sin q1(t) cos q1(t) 0 (103)
With DHangularjacobian, see SectionA.14, the part Jωof the Jacobian for the
four 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 the Jacobian, 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 Jacobian
In this section the examples with the linear and angular Jacobian in Maple are collected and the procedures DHlinearjacobian and DHangularjacobian are combined to DHjacobian, which calculates the whole Jacobian directly. See also SectionA.15 for a description in Maple code.
7.4.1 How to do in Maple
Since the resulting symbolic Jacobian for a robot with 6 links is very large (about 9 pages of Maple output code), a numerical example is somewhat more illustrative here. First the links are defined, the robot model is created and then the Jacobian can be derived using DHjacobian.
link1 := DHlink(0, 0.475, 0.15, -Pi/2, r); link2 := DHlink(-Pi/2, 0, 0.6, 0, r ); link3 := DHlink(0, 0, 0.12, -Pi/2, r); link4 := DHlink(0, 0.72, 0, Pi/2, r); link5 := DHlink(0, 0, 0, -Pi/2, r); link6 := DHlink(Pi, 0.085, 0, 0, r);
myrobot := DHrobot(link1, link2, link3, link4, link5, link6); J := DHjacobian(myrobot);
The resulting Jacobian becomes
J06= 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) of the Jacobian, which of course are the same as the linear and angular part of the matrix above.
If we for a moment pretend that the robot consists of only prismatic joints, we only change r to p in the link type definitions above in the Maple code. The corresponding Jacobian then becomes
J06= 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 angular part now only consists of zeros. The linear part also avoids the more complicated cross product zi−1× (dn0 − d
i−1
0 ), where n is the number of links of the robot.
7.5
Discussion about the Jacobian
Since the Jacobian is an important quantity in robot modelling, analysis and control, see also Section 7.1, there are of course many interesting things to know about it. In this section some of the ideas and thoughts about the Jacobian are discussed. The main source in this section is [17] if nothing else is mentioned.
