Abb Kinematics

Exercise 1a: Forward Kinematics of the
ABB IRB 120

Marco Hutter, Michael Blosch, Dario Bellicoso, Samuel Bachmann
September 28, 2016
Abstract
The aim of this exercise is to calculate the forward and inverse kinematics
of an ABB robot arm. In doing so, you will practice the use of different rep-
resentations of the end-effectors orientation as well as how to check whether
your implementations are correct. Essentially, the task is to implement the
tools for computing the forward and inverse kinematics in using symbolic and
numerical computations in MATLAB. A separate MATLAB script will be
provided for the 3D visualization of the robot arm.
1 Introduction
The following exercise is based on an ABB IRB 120 depicted in figure 2. It is a 6-
link robotic manipulator with a fixed base. During the exercise you will implement
several different MATLAB functions, which, you should test carefully since the
following tasks are often dependent on them. To help you with this, we have
provided the script prototypes at bitbucket.org/ethz-asl-lr/robotdynamics_
exercise_1a together with a visualizer of the manipulator.
1
2 Forward Kinematics
Figure 1: ABB IRB 120 with coordinate systems and joints
Throughout this document, we will employ I for denoting the inertial world coor-
dinate system (coordinate system P0 in figure 2) and E for the coordinate system
attached to the end-effector (coordinate system P6 in figure 2).
Exercise 2.1
Define a vector q of generalized coordinates to describe the configuration of the ABB
IRB120. Recall that generalized coordinates should be complete and independent.
The former property means that they should fully described the configuration of
the robot while at the same time comprising a minimal set of coordinates. The
latter property refers to the fact that the each generalized coordinate must not be
a function of any of the others.
Solution 2.1
The generalized coordinates can be chosen as the single joint angles between subse-
quent links. Any other complete and independent linear combination of the single
joint angles is also a valid solution.
T
q = (q1 , . . . , q6 ) R6 (1)
Exercise 2.2
Compute the homogeneous transformations matrices Tk1,k (qk ), k = 1, . . . , 6.
Additionally, find the constant homogeneous transformations between the inertial
frame and frame 0 (TI0 ) and between frame 6 and the end-effector frame (T6E ).
Please implement the following functions:
1 function TI0 = getTransformI0()

2 % Input: void
3 % Output: homogeneous transformation Matrix from the inertial ...
frame 0 to frame I. T I0
2
4 end
5
6 function T01 = jointToTransform01(q)
7 % Input: joint angles
8 % Output: homogeneous transformation Matrix from frame 1 to frame ...
0. T 01
9 end
10
1. T 12
14 end
15
2. T 23
19 end
20
3. T 34
24 end
25
4. T 45
29 end
30
5. T 56
34 end
35
36 function T6E = getTransform6E()
37 % Input: void
38 % Output: homogeneous transformation Matrix from the endeffector ...
frame E to frame 6. T 6E
39 end
Solution 2.2
Remember that a homogeneous transformation matrix is expressed in the form

Chk (qk ) h rhk (qk )
Thk (qk ) = . (2)
013 1
For the ABB IRB 120, each Thk (qk ) is composed by an elementary rotation a single
joint axis and a translation defined by the manipulator kinematic parameters. By
defining the elementary rotations matrices about each axis as

cos() sin() 0
Cz () = sin() cos() 0 (3)
0 0 1

cos() 0 sin()
Cy () = 0 1 0 (4)
sin() 0 cos()

1 0 0
Cx () = 0 cos() sin() , (5)
0 sin() cos()
3
one can write
Cz (q1 ) 0 r01 (q1 )
T01 (q1 ) = (6)
013 1

Cy (q2 ) 1 r12 (q2 )
T12 (2 ) = (7)
013 1

Cy (q3 ) 2 r23 (q3 )
T23 (q3 ) = (8)
013 1

Cx (q4 ) 3 r34 (q4 )
T34 (q4 ) = (9)
013 1

Cy (q5 ) 4 r45 (q5 )
T45 (q5 ) = (10)
013 1

Cx (q6 ) 5 r56 (q6 )
T56 (q6 ) = . (11)
013 1
Finally, the constant homogeneous transformations TI0 and T6E are simply the
identity matrix I44 .
1 function TI0 = getTransformI0()

2 % Input: void
3 % Output: homogeneous transformation Matrix from the inertial ...
frame 0 to frame I. T I0
4 TI0 = eye(4);
5 end
6
8 % Input: joint angle
0. T 01
10 if (length(q)>1)
11 q = q(1);
12 end
13 T01 = [cos(q), sin(q), 0, 0;
14 sin(q), cos(q), 0, 0;
15 0, 0, 1, 0.145;
16 0, 0, 0, 1];
17 end
18
1. T 12
22 if (length(q)>1)
23 q = q(2);
24 end
25 T12 = [cos(q), 0, sin(q), 0;
26 0, 1, 0, 0;
27 sin(q), 0, cos(q), 0.145;
28 0, 0, 0, 1];
29 end
30
2. T 23
34 if (length(q)>1)
35 q = q(3);
36 end
37 T23 = [cos(q), 0, sin(q), 0;
38 0, 1, 0, 0;
4
39 sin(q), 0, cos(q), 0.270;
40 0, 0, 0, 1];
41 end
42
3. T 34
46 if (length(q)>1)
47 q = q(4);
48 end
49 T34 = [1, 0, 0, 0.134;
50 0, cos(q), sin(q), 0;
51 0, sin(q), cos(q), 0.070;
52 0, 0, 0, 1];
53 end
54
4. T 45
58 if (length(q)>1)
59 q = q(5);
60 end
61 T45 = [cos(q), 0, sin(q), 0.168;
62 0, 1, 0, 0;
63 sin(q), 0, cos(q), 0;
64 0, 0, 0, 1];
65 end
66
6. T 56
70 if (length(q)>1)
71 q = q(6);
72 end
73 T56 = [1, 0, 0, 0.072;
74 0, cos(q), sin(q), 0;
75 0, sin(q), cos(q), 0;
76 0, 0, 0, 1];
77 end
78
79 function T6E = getTransform6E()
80 % Input: void
81 % Output: homogeneous transformation Matrix from the endeffector ...
frame E to frame 6. T 6E
82 T6E = eye(4);
83 end
Exercise 2.3
Find the end-effector position vector I rIE = I rIE (q). Please implement the follow-
ing function:
1 function I r IE = jointToPosition(q)
3 % Output: position of endeffector w.r.t. inertial frame. I r IE
4 end
Solution 2.3
The end-effector position is given by the direct kinematics, represented in matrix
form by the homogeneous transformation TIE (q), which can be found by successive
concatenation of coordinate frame transformations.
5
6
!
Y CIE I rIE (q)
TIE (q) = TI0 Tk1,k T6E = (12)
013 1
k=1
The end-effector position can then be found by selecting the fourth column of
TIE (q).

0
r (q) 0
= TIE (q) (13)
1 0
1
1 function I r IE = jointToPosition(q)
3 % Output: position of endeffector w.r.t. inertial frame. I r IE
4 T IE = getTransformI0()*...
5 jointToTransform01(q)*...
11 getTransform6E();
12 I r IE = T IE(1:3,4);
13 end
Exercise 2.4

/6

/6

/6
What is the end-effector position for q = ? Use matlab to display it.

/6

/6
/6
Solution 2.4
From the direct kinematics equations found earlier, it is:

0.2948
I rIE = I rIE (q) = 0.1910 . (14)

0.2277
Exercise 2.5
Find the end-effector rotation matrix CIE = CIE (q). Please implement the fol-
lowing function:
1 function C IE = jointToRotMat(q)
3 % Output: rotation matrix which projects a vector defined in the
4 % endeffector frame E to the inertial frame I, C IE.
5 end
6
Solution 2.5
From the structure of the direct kinematics equations found earlier, it follows that
the end-effector rotation matrix is obtained by extracting the first three rows and
the first three columns from TIE (q). This operation can be compactly written in
matrix form:

I33
I
CIE (q) = 33 031 TIE (q) . (15)
013
1 function C IE = jointToRotMat(q)
3 % Output: rotation matrix which projects a vector defined in the
4 % endeffector frame E to the inertial frame I, C IE.
5 T IE = getTransformI0() * ...
6 jointToTransform01(q) * ...
12 getTransform6E();
13 C IE = T IE(1:3,1:3);
14 end
Exercise 2.6
Find the quaternion representing the attitude of the end-effector IE = IE (q).
Please also implement the following function:
Two functions for converting from quaternion to rotation matrices and vice-
versa. Test these by converting from quaternions to rotation matrices and
back to quaternions.
The quaternion multiplication q p
The passive rotation of a vector with a given quaternion. This can be im-
plemented in different ways which can be tested with respect to each other.
The easiest way is to transform the quaternion to the corresponding rotation
matrix (by using the function from above) and then multiply the matrix with
the vector to be rotated.
Also check that your two representations for the end-effector orientation match with
each other. In total you should write the following five functions:
1 function quat = jointToQuat(q)

3 % Output: quaternion representing the orientation of the ...
endeffector (quat IE)
4 end
5
6 function C = quatToRotMat(quat)
7 % Input: quaternion [w x y z]
8 % Output: corresponding rotation matrix
9 end
10
11 function quat = rotMatToQuat(C)
12 % Input: rotation matrix
13 % Output: corresponding quaternion [w x y z]
14 end
15
7
16 function quat = quatMult(quat AB,quat BC)
17 % Input: two quaternions to be multiplied
18 % Output: output of the multiplication
19 end
20
21 function A r = rotVecWithQuat(quat AB,B r)
22 % Input: the orientation quaternion and the coordinate of the ...
vector to be mapped
23 % Output: the coordinates of the vector in the target frame
24 end
Solution 2.6
6
!
Y
CIE (q) = CI0 Ck1,k C6E (16)
k=1

c11 +c22 + c33 + 1
sgn(c32 c23 ) c11 c22 c33 + 1
(q) = 12 (17)
sgn(c13 c31 ) c22 c33 c11 + 1

sgn(c21 c12 ) c33 c11 c22 + 1
where cij = C(i, j).
1 function quat = jointToQuat(quat)

3 % Output: quaternion representing the orientation of the endeffector
4 % q IE.
5 C = jointToRotMat(q);
6 quat = rotMatToQuat(C);
7 end
8
9 function C = quatToRotMat(quat)
10 % Input: quaternion [w x y z]
11 % Output: corresponding rotation matrix
12
13 % Extract the scalar part.
14 quat w = quat(1);
15
16 % Extract the vector part.
17 quat n = quat(2:4);
18
19 % Map the unit quaternion to a rotation matrix.
20 C = (2* quat w21)*eye(3) + 2.0* quat w *skewMatrix(quat n) + ...
2.0*(quat n *quat n');
21 end
22
23 function quat = rotMatToQuat(C)
24 % Input: rotation matrix
25 % Output: corresponding quaternion [w x y z]
26 quat = 0.5*[sqrt((1+trace(C)));
27 sign(C(3,2)C(2,3)) * sqrt(C(1,1) C(2,2) C(3,3) + 1);
28 sign(C(1,3)C(3,1)) * sqrt(C(2,2) C(3,3) C(1,1) + 1);
29 sign(C(2,1)C(1,2)) * sqrt(C(3,3) C(1,1) C(2,2) + ...
1)];
30 end
31
32 function quat AC = quatMult(quat AB,quat BC)
33 % Input: two quaternions to be multiplied
34 % Output: output of the multiplication
35 q = quat AB;
36 p = quat BC;
37
38 q w = q(1); q n = q(2:4);
39 p w = p(1); p n = p(2:4);
8
40
41 skew q n = [0 q n(3) q n(2);
42 q n(3) 0 q n(1);
43 q n(2) q n(1) 0];
44
45 quat AC = [q w * p w q n'* p n;
46 q w * p n + p w * q n + skew q n * p n];
47 end
48
49 function A r = rotVecWithQuat(quat AB,B r)
50 % Input: the orientation quaternion and the coordinate of the ...
vector to be mapped
51 % Output: the coordinates of the vector in the target frame
52 C AB = quatToRotMat(quat AB);
53 A r = C AB * B r;
54 end

Abb Kinematics

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Abb Kinematics

Uploaded by

Copyright:

Available Formats

Exercise 1a: Forward Kinematics of the

ABB IRB 120

Figure 1: ABB IRB 120 with coordinate systems and joints

1 function TI0 = getTransformI0()

1 function TI0 = getTransformI0()

1 function quat = jointToQuat(q)

1 function quat = jointToQuat(quat)

You might also like