Exercise 1: Kinematics of the ABB IRB 120

Marco Hutter, Michael Blosch, Dario Bellicoso, Samuel Bachmann

October 2, 2015

Abstract
In this exercise you learn how to calculate the forward and inverse kine-
matics of an ABB robot arm. A Matlab visualization of the robot arm is
provided. You will have to implement the tools to compute the forward and
inverse kinematics in your Matlab script. During this you will exercise how to
work with different representations of the end-effector orientation and how to
check whether your implementations are correct. In the end, you will apply
inverse-kinematics in order to have the robot follow a desired path with the
end-effector.

1 Introduction
The following exercise is for the ABB IRB 120 depicted in figure 2. It is a 6-link
robotic manipulator with a fixed base. During the exercise you will implement
a lot of different Matlab functions - test them 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_exercise1 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 the generalized ~ coordinates for the ABB IRB120. Please remember that
generalized coordinates should be complete (the configuration of the robot should
be fully specified by the generalized coordinates) and independent (no coordinate
is a pure function 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
~ = (1 , . . . , 6 ) R61 (1)

Exercise 2.2
Find the homogenous transformations matrices Tk1,k (k ), k = 1, . . . , 6. Addi-
tionally, 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
4 end
5

2
 6 function T01 = jointToTransform01(x)
7 % Input: joint angles
8 % Output: homogeneous transformation Matrix from frame 1 to frame ...
0. T 01
9 end
10
11 function T12 = jointToTransform12(x)
12 % Input: joint angles
13 % Output: homogeneous transformation Matrix from frame 2 to frame ...
1. T 12
14 end
15
16 function T23 = jointToTransform23(x)
17 % Input: joint angles
18 % Output: homogeneous transformation Matrix from frame 3 to frame ...
2. T 23
19 end
20
21 function T34 = jointToTransform34(x)
22 % Input: joint angles
23 % Output: homogeneous transformation Matrix from frame 4 to frame ...
3. T 34
24 end
25
26 function T45 = jointToTransform45(x)
27 % Input: joint angles
28 % Output: homogeneous transformation Matrix from frame 5 to frame ...
4. T 45
29 end
30
31 function T56 = jointToTransform56(x)
32 % Input: joint angles
33 % Output: homogeneous transformation Matrix from frame 6 to frame ...
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:
 
Rhk (k ) h~
rhk (k )
Thk (k ) = ~013 (2)
1
For the ABB IRB 120, each Thk (k ) 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
Rz () = sin() cos() 0 (3)
0 0 1

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

1 0 0
Rx () = 0 cos() sin() (5)
0 sin() cos()
one can write:

3
  
Rz (1 ) 0~
r01 (1 )
T01 (1 ) = ~013 (6)
1
 
Ry (2 ) 1~
r12 (2 )
T12 (2 ) = ~ (7)
013 1
 
Ry (3 ) 2~
r23 (3 )
T23 (3 ) = ~ (8)
013 1
 
Rx (4 ) 3~
r34 (4 )
T34 (4 ) = ~ (9)
013 1
 
Ry (5 ) 4~
r45 (5 )
T45 (5 ) = ~ (10)
013 1
 
Rx (6 ) 5~
r56 (6 )
T56 (6 ) = ~ (11)
013 1
Finally, the constant homogeneous transformations TIE 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
7 function T01 = jointToTransform01(x)
8 % Input: joint angle
9 % Output: homogeneous transformation Matrix from frame 1 to frame ...
0. T 01
10 T01 = [cos(x), sin(x), 0, 0;
11 sin(x), cos(x), 0, 0;
12 0, 0, 1, 0.145;
13 0, 0, 0, 1];
14 end
15
16 function T12 = jointToTransform12(x)
17 % Input: joint angle
18 % Output: homogeneous transformation Matrix from frame 2 to frame ...
1. T 12
19 T12 = [cos(x), 0, sin(x), 0;
20 0, 1, 0, 0;
21 sin(x), 0, cos(x), 0.145;
22 0, 0, 0, 1];
23 end
24
25 function T23 = jointToTransform23(x)
26 % Input: joint angle
27 % Output: homogeneous transformation Matrix from frame 3 to frame ...
2. T 23
28 T23 = [cos(x), 0, sin(x), 0;
29 0, 1, 0, 0;
30 sin(x), 0, cos(x), 0.270;
31 0, 0, 0, 1];
32 end
33
34 function T34 = jointToTransform34(x)
35 % Input: joint angle
36 % Output: homogeneous transformation Matrix from frame 4 to frame ...
3. T 34
37 T34 = [1, 0, 0, 0.134;

4
38 0, cos(x), sin(x), 0;
39 0, sin(x), cos(x), 0.070;
40 0, 0, 0, 1];
41 end
42
43 function T45 = jointToTransform45(x)
44 % Input: joint angle
45 % Output: homogeneous transformation Matrix from frame 5 to frame ...
4. T 45
46 T45 = [cos(x), 0, sin(x), 0.168;
47 0, 1, 0, 0;
48 sin(x), 0, cos(x), 0;
49 0, 0, 0, 1];
50 end
51
52 function T56 = jointToTransform56(x)
53 % Input: joint angle
54 % Output: homogeneous transformation Matrix from frame 5 to frame ...
6. T 56
55 T56 = [1, 0, 0, 0.072;
56 0, cos(x), sin(x), 0;
57 0, sin(x), cos(x), 0;
58 0, 0, 0, 1];
59 end
60
61 function T6E = getTransform6E()
62 % Input: void
63 % Output: homogeneous transformation Matrix from the endeffector ...
frame E to frame 6. T 6E
64 T6E = eye(4);
65 end

Exercise 2.3
~
Find the end-effector position I ~rIE = I ~rIE (). Please implement the following
function:

1 function r = jointToPosition(x)
2 % Input: joint angles
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 (),~ which can be found by successive
concatenation of coordinate frame transformations.
! "  #
6
~ = TI0
Y R IE I ~
rIE ~
TIE () Tk1,k T6E = (12)
k=1
~013 1
The end-effector position can then be found by selecting the fourth column of
~
TIE ().

"  # 0
~r ~ 0
~
= TIE () 0 (13)
1
1

1 function r = jointToPosition(x)
2 % Input: joint angles

5
 3 % Output: position of endeffector w.r.t. inertial frame. I r IE
4
5 TI0 = getTransformI0();
6 T01 = jointToTransform01(x(1));
7 T12 = jointToTransform12(x(2));
8 T23 = jointToTransform23(x(3));
9 T34 = jointToTransform23(x(4));
10 T45 = jointToTransform23(x(5));
11 T56 = jointToTransform23(x(6));
12 T6E = getTransform6E();
13
14 TIE = TI0*T01*T12*T23*T34*T45*T56*T6E;
15 r = TIE(1:3,4);
16 end

Exercise 2.4

/6

/6

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

/6

/6
/6

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

  0.2948
rIE = I ~rIE ~ = 0.1910 .
I~ (14)
0.2277

Exercise 2.5
 
Find the analytical expression of the end-effector linear velocity I ~vIE = I ~vIE ~ =
 
dI ~rIE ~
.
dt
Solution 2.5

Figure 2: Linear velocity of a point in a rototranslating frame.

Consider the coordinate frames shown in Fig.2. Frame 0 is fixed, while frame 1 has
a linear velocity 0 v01 and angular velocity 0 1 . Consider a point P that is fixed in

6
 P
frame 1. The linear velocity 0 v02 of point P with respect to the fixed frame 0 is
given by:
P
0 v02 = 0 v01 + 0 r12 + 0 1 0 r12 . (15)
If point P is fixed in frame 1, it is 0 r12 = 0. With this result in mind, consider now
a planar two link robot arm with two revolute joints. The coordinate frames are
chosen as in Fig.2. Reasoning as in Eq.(??), the linear velocity at the end of the
kinematic chain can be found by propagating the velocity contributions from the
fixed frame 0. Hence, one has:

0 v01 = 0 v0 + 0 0 0 r01
0 v02 = 0 v01 + 0 1 0 r12 (16)
0 v0E = 0 v02 + 0 2 0 r2E

Figure 3: The kinematic structure of a planar two link robot arm.

Combining these results with the fact the frame 0 is fixed (i.e. 0 0 = 0, 0 v0 = 0),
the end-effector linear velocity is given by:

0 v0E = 0 1 0 r12 + 0 2 0 r2E (17)

This result can be extended to the case of the ABB IRB 120, yielding:

I~
vIE = I 1 I r12 + I 2 I r23 + + I 5 I r56 + I E I r6E
(18)
= I ~v01 + I ~v12 + + I ~v56 + I ~v6E

Exercise 2.6
 
Find the end-effector rotation matrix RIE = RIE ~ . Please implement the fol-
lowing function:

1 function R = jointToRotMat(x)
2 % Input: joint angles
3 % Output: rotation Matrix of the endeffector. R IE
4 end

7
Solution 2.6
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
~ This operation can be compactly written in
the first three columns from TIE ().
matrix form:

I~33
    
~ ~ ~ ~

RIE = I33 031 TIE () ~ . (19)
013

1 function R = jointToRotMat(x)
2 % Input: joint angles
3 % Output: rotation Matrix of the endeffector. R EI
4
5 TI0 = getTransformI0();
6 T01 = jointToTransform01(x(1));
7 T12 = jointToTransform12(x(2));
8 T23 = jointToTransform23(x(3));
9 T34 = jointToTransform23(x(4));
10 T45 = jointToTransform23(x(5));
11 T56 = jointToTransform23(x(6));
12 T6E = getTransform6E();
13
14 TIE = TI0*T01*T12*T23*T34*T45*T56*T6E;
15 R = T IE(1:3,1:3);
16 end

Exercise 2.7
 
Find the quaternion representing the attitude of the end-effector q IE = q IE ~ .
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 (unfortunately the quaternion implemen-
tation in the aerospace toolbox of Matlab uses Hamilton convention, hence
use the definition from the lecture slides, where the JPL [1] convention has
been used).
The function rotating a vector with a given quaternion. This can be imple-
mented in different ways which can be tested with respect to each other. The
easiest way is to transform the quaternion to the corresponding rotation ma-
trix (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 q = jointToQuat(x)
2 % Input: joint angles
3 % Output: quaternion representing the orientation of the ...
endeffector. q EI
4 end
5
6 function R = quatToRotMat(q)
7 % Input: quaternion [w x y z]
8 % Output: corresponding rotation matrix

8
 9 end
10
11 function q = rotMatToQuat(R)
12 % Input: rotation matrix
13 % Output: corresponding quaternion [w x y z]
14 end
15
16 function q AC = quatMult(q AB,q BC)
17 % Input: two quaternions to be multiplied
18 % Output: output of the multiplication
19 end
20
21 function B r = rotVecWithQuat(q BA,A 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.7
6
!
  Y
RIE ~ = RI0 Rk1,k R6E (20)
k=1
p
(1 + T ) /4
(R23 R32 ) /(4 q 1 )
 
q ~ =
(R31 R13 ) /(4 q 1 ) (21)
(R12 R21 ) /(4 q 1 )
where T = Trace(RIE ).

1 function q = jointToQuat(x)
2 % Input: joint angles
3 % Output: quaternion representing the orientation of the ...
endeffector. q EI
4
5 R = jointToRotMat(x);
6 q = rotMatToQuat(R);
7 end
8
9 function R = quatToRotMat(q)
10 % Input: quaternion [w x y z]
11 % Output: corresponding rotation matrix
12
13 q = q(:);
14 R = (2*q(1)21)*eye(3) 2*q(1)*skewMatrix(q(2:end)) + ...
2*q(2:end)*q(2:end)';
15 end
16
17 function q = rotMatToQuat(R)
18 % Input: rotation matrix
19 % Output: corresponding quaternion [w x y z]
20
21 % JPL
22 q1 = sqrt((1+trace(R))/4);
23 q = [q1;
24 (R(2,3)R(3,2))/(4*q1);
25 (R(3,1)R(1,3))/(4*q1);
26 (R(1,2)R(2,1))/(4*q1)];
27 end
28
29 function q AC = quatMult(q AB,q BC)
30 % Input: two quaternions to be multiplied
31 % Output: output of the multiplication
32

9
33 q = q AB;
34 p = q BC;
35
36 % JPL
37 q AC = [q(2)*p(2) q(3)*p(3) q(4)*p(4) + q(1)*p(1);
38 q(1)*p(2) + q(4)*p(3) q(3)*p(4) + q(2)*p(1);
39 q(4)*p(2) + q(1)*p(3) + q(2)*p(4) + q(3)*p(1);
40 q(3)*p(2) q(2)*p(3) + q(1)*p(4) + q(4)*p(1)];
41 end
42
43 function B r = rotVecWithQuat(q BA,A r)
44 % Input: the orientation quaternion and the coordinate of the ...
vector to be mapped
45 % Output: the coordinates of the vector in the target frame
46
47 R BA = quatToRotMat(q BA);
48 B r = R BA * A r;
49 end

Exercise 2.8
 
Find the analytical expression of the end-effector rotational velocity I ~ IE ~ .
~ IE = I

Solution 2.8
The end-effector rotational velocity I
~ IE is obtained by summing the single joint
velocity contributions:

I
~ IE = I
~ I0 + I ~ 12 + + I
~ 01 + I ~ 56 + I
~ 6E (22)

10
3 Inverse Kinematics
Exercise 3.1
Derive the Jacobians of the end-effector position and orientation based on the linear
and rotational velocity derived above in exercise 2. The Jacobians should depend
on the minimal coordinates only. Remember that:

I~
vIE ~ ~
=JP () (23)

I
~ IE ~ ~
=JR () (24)

Please implement the following two functions:

1 function J P = jointToPosJac(x)
2 % Input: joint angles
3 % Output: Jacobian of the endeffector position
4 end
5
6 function J R = jointToRotJac(x)
7 % Input: joint angles
8 % Output: Jacobian of the endeffector orientation
9 end

Solution 3.1
The translation and rotation Jacobians can be evaluated starting from the results
that were obtained in the previous exercises. By combining the analytical expres-
sions of the linear and angular end-effector velocities, one has:

I~
vIE = I ~v01 + I ~v12 + + I ~v56 + I ~v6E
= I 1 I r12 + I 2 I r23 + + I 5 I r56 + I E I r6E
= I 1 (I rI2 I rI1 ) + I 2 (I rI3 I rI2 ) + + I 5 (I rI6 I rI5 )
= (I 0 + I 01 ) (I rI2 I rI1 ) (25)
+ (I 1 + I 12 ) (I rI3 I rI2 )
+ ...
+ (I 5 + I 56 ) (I rIE I rI6 )

At this point it is worth noticing that it is I k1,k = I k k , where I k = RIk k k

and k k represents the axis around which joint k can rotate. Recalling that:

I k = I k1 + I k1,k , (26)
one has:

I~
vIE = (I 0 + I 01 ) (I rI2 I rI1 )
+ (I 1 + I 12 ) (I rI3 I rI2 )
+ ...
+ (I 5 + I 56 ) (I rIE I rI6 )
(27)
= (I 1 1 ) (I rI2 I rI1 )
+ (I 1 1 + I 2 2 ) (I rI3 I rI2 )
+ ...
+ (I 1 1 + + I 6 6 ) (I rIE I rI6 )

11
Expanding and reordering the terms in the last equation, one has:

I~
vIE = I 1 1 (I rIE I rI1 )
+ I 2 2 (I rIE I rI2 )
, (28)
+ ...
+ I 6 6 (I rIE I rI6 )
which, rewritten in matrix from, gives:

  1
I~
vIE = I 1 (I rIE I rI1 ) ... I 6 (I rIE I rI6 ) . . .
6 (29)

~
~ ,
= JP ()
~ is the translation Jacobian matrix that projects a vector from the joint
where JP ()
velocity space to the cartesian linear velocity space.
Using the results obtained by solving Exercise 8, and taking into account that I
~ I0
and I
~ 6E are both equal to zero, one has:

I
~ IE = I
~ 01 + I ~ 12 + + I ~ 56
= I ~ 6 6
~ 2 2 + + I
~ 1 1 + I
i , (30)
~
h
= I ~ 1

~
I 2 . . .
~
I 6

~ ~
= JR ()

~ is the rotation Jacobian matrix that projects a vector in the joint

where JR ()
velocity space to the cartesian angular velocity space.
We now inspect a second method to derive the rotation Jacobian. It can be shown
~ is given by:
that the time derivative of the rotation matrix RIE ()

~ = S(I
RIE () ~
~ IE ) RIE (), (31)
where S(I
~ IE ) is the skew matrix operator defined as:

0 z y
) = z
S(~ 0 x . (32)
y x 0
Using the chain rule, one can also write:

R()~
~ =
R() ~ (33)
~
Combining the last two equations and using the orthonormality property of rotation
matrices, one has:

S(I ~ RIE ()
~ IE ) = RIE () ~ T. (34)
The end-effector velocity I
~ IE can be easily extracted from S(I
~ IE ). The rotational
Jacobian can now be evaluated as:

~ = I
JR ()
~ IE
(35)

12
1 function J P = jointToPosJac(x)
2 % Input: joint angles
3 % Output: Jacobian of the endeffector position
4 T I0 = getTransformI0();
5 T 01 = jointToTransform01(x(1));
6 T 12 = jointToTransform12(x(2));
7 T 23 = jointToTransform23(x(3));
8 T 34 = jointToTransform34(x(4));
9 T 45 = jointToTransform45(x(5));
10 T 56 = jointToTransform56(x(6));
11
12 T I1 = T I0 * T 01;
13 T I2 = T I1 * T 12;
14 T I3 = T I2 * T 23;
15 T I4 = T I3 * T 34;
16 T I5 = T I4 * T 45;
17 T I6 = T I5 * T 56;
18
19 R I1 = T I1(1:3,1:3);
20 R I2 = T I2(1:3,1:3);
21 R I3 = T I3(1:3,1:3);
22 R I4 = T I4(1:3,1:3);
23 R I5 = T I5(1:3,1:3);
24 R I6 = T I6(1:3,1:3);
25
26 r I I1 = T I1(1:3,4);
27 r I I2 = T I2(1:3,4);
28 r I I3 = T I3(1:3,4);
29 r I I4 = T I4(1:3,4);
30 r I I5 = T I5(1:3,4);
31 r I I6 = T I6(1:3,4);
32
33 omega hat 1 = [0 0 1]';
34 omega hat 2 = [0 1 0]';
35 omega hat 3 = [0 1 0]';
36 omega hat 4 = [1 0 0]';
37 omega hat 5 = [0 1 0]';
38 omega hat 6 = [1 0 0]';
39
40 r I IE = jointToPosition(x);
41
42 J P = [ cross(R I1 * omega hat 1, r I IE r I I1) ...
43 cross(R I2 * omega hat 2, r I IE r I I2) ...
44 cross(R I3 * omega hat 3, r I IE r I I3) ...
45 cross(R I4 * omega hat 4, r I IE r I I4) ...
46 cross(R I5 * omega hat 5, r I IE r I I5) ...
47 cross(R I6 * omega hat 6, r I IE r I I6) ...
48 ];
49 end
50
51 function J R = jointToRotJac(x)
52 % Input: joint angles
53 % Output: Jacobian of the endeffector orientation
54 T I0 = getTransformI0();
55 T 01 = jointToTransform01(x(1));
56 T 12 = jointToTransform12(x(2));
57 T 23 = jointToTransform23(x(3));
58 T 34 = jointToTransform34(x(4));
59 T 45 = jointToTransform45(x(5));
60 T 56 = jointToTransform56(x(6));
61
62 T I1 = T I0 * T 01;
63 T I2 = T I1 * T 12;
64 T I3 = T I2 * T 23;
65 T I4 = T I3 * T 34;
66 T I5 = T I4 * T 45;
67 T I6 = T I5 * T 56;

13
68
69 R I1 = T I1(1:3,1:3);
70 R I2 = T I2(1:3,1:3);
71 R I3 = T I3(1:3,1:3);
72 R I4 = T I4(1:3,1:3);
73 R I5 = T I5(1:3,1:3);
74 R I6 = T I6(1:3,1:3);
75
76 omega hat 1 = [0 0 1]';
77 omega hat 2 = [0 1 0]';
78 omega hat 3 = [0 1 0]';
79 omega hat 4 = [1 0 0]';
80 omega hat 5 = [0 1 0]';
81 omega hat 6 = [1 0 0]';
82
83 J R = [ R I1 * omega hat 1 ...
84 R I2 * omega hat 2 ...
85 R I3 * omega hat 3 ...
86 R I4 * omega hat 4 ...
87 R I5 * omega hat 5 ...
88 R I6 * omega hat 6 ...
89 ];
90 end

Exercise 3.2
Find a set of minimal coordinates such that the end-effector position is at ~r =
 T
0.5649 0 0.5509 while its orientation is kept as identity. To this end you
will have to implement an iterative inverse kinematics algorithm in Matlab. The
algorithm can be summarized as follows:
1. Initialize the iterations to i = 0 and start with some initial guess ~0
2. Compute difference between desired values and actual values ~ri = ~r ~r(~i ),
and q i = q q(~i ), with q = (1, 0, 0, 0).
3. Construct the following system of linear equations and solve for ~i .
 " #
JP (~i )

~ri
= ~i (36)
q i (q(~i ))JR (~i )

4. Apply the correction ~i onto ~i with ~i+1 = ~i + ~i

5. Check if the correction is smaller then a specific threshold , if not set i := i+1
and continue with item 2.
Please implement your solution in the following function. You will also need to
~ which maps the rotational velocity to the quaternion
implement the function (q())
derivative.

1 function X = xiMatrix(q)
2 % Input: quaternion
3 % Output: 4x3 matrix mapping local rotational velocities to ...
quaternion derivatives
4 end
5
6 function x = inverseKinematics(r des,q des,x init,epsilon)
7 % Input: desired endeffector position, desired endeffector ...
orientation (quaternion), initial guess for joint angles, ...
threshold for stoppingcriterion
8 % Output: joint angles with match desired endeffector position ...
and orientation
9 end

14
Solution 3.2
Recall that the mapping between the time derivative of the quaternion q EI and the
local angular velocity E
~ IE is given by:
1 ~ E
q EI = (q EI ()) ~ IE , (37)
2
where
 
~ = q EI
(q EI ()) (38)
qEI,0 I33 + q xBI
Since the rotation Jacobian that we derived earlier maps joint velocities to I
~ IE ,
we will need multiply by the appropriate rotation matrix to obtain E
~ IE .

1 function X = xiMatrix(q)
2 % Input: quaternion
3 % Output: 4x3 matrix mapping local rotational velocities to ...
quaternion derivatives
4
5 % Make sure that q is a column
6 q = q(:);
7 X = 0.5*[q(2:end)';
8 q(1)*eye(3)+skewMatrix(q(2:end))];
9 end
10
11 function x = inverseKinematics(r des,q des IE,x init,epsilon)
12 % Input: desired endeffector position, desired endeffector ...
orientation (quaternion), initial guess for joint angles, ...
threshold for stoppingcriterion
13 % Output: joint angles which match desired endeffector position ...
and orientation
14
15 %% Setup
16 iterations = 0;
17 dth = inf(length(x init),1);
18 max iterations = 1000;
19
20 q des IE = q des IE(:);
21 r des = r des(:);
22 x init = x init(:);
23
24 q des EI = invertQuat(q des IE);
25
26 % Initialize with initial guess
27 theta = x init;
28
29 % Iterate until terminating condition
30 while (norm(dth)>epsilon && iterations < max iterations)
31
32 q EI = invertQuat(jointToQuat(theta));
33
34 dr = r des jointToPosition(theta);
35 dq = q des EIq EI;
36
37 R IE = jointToRotMat(theta);
38
39 A = [jointToPosJac(theta);
40 xiMatrix(q EI)* R IE'*jointToRotJac(theta)];
41
42 dth = A\[dr; dq];
43
44 % Update
45 theta = theta + dth;
46 iterations = iterations+1;

15
47
48 end
49
50 x = theta;
51
52 pos error = r des jointToPosition(x);
53 quat error = 1 norm(quatMult(q des EI, ...
quatInverse(jointToQuat(x))));
54
55 fprintf('Inverse kinematics terminated after %d ...
iterations.\n',iterations);
56 fprintf('Position error: %e.\n',norm(pos error));
57 fprintf('Attitude error: %e.\n',quat error);
58
59 end
60
61 function q inv = invertQuat(q)
62 % Input: a unit quaternion
63 % Output: the inverse of the input quaternion
64 q = q(:);
65 q inv = [q(1); q(2:end)];
66 q inv = q inv/norm(q inv);
67 end
68
69 function S = skewMatrix(x)
70 % Input: a vector in R3
71 % Output: the skew matrix S(x)
72 S = [0 x(3) x(2);
73 x(3) 0 x(1);
74 x(2) x(1) 0];
75 end

Exercise 3.3
So far we always represented the orientation of the end-effector by means of the full
rotation matrix or quaternions. This has the advantage that subsequent relative
orientations can just be concatenated by multiplying the matrices or quaternions to-
gether. Now, in order to get a more compact and human-readable representation of
the end-effector orientation we will express it by means of Euler-angles
~ = (, , )
with the sequence z-y-x. To this end, please reimplement the inverse kinematics al-
gorithm by using a new representation transformation zyx (q()) ~ for the rotation
Jacobian. You should implement the following functions:

1 function a = jointToEulAng(x)
2 % Input: joint angles
3 % Output: corresponding euler angles with zyx sequence
4 end
5
6 function X = xiMatrixEuler(a)
7 % Input: zyx Euler angles
8 % Output: 3x3 matrix mapping global rotational velocities to ...
Euler angle derivatives
9 end
10
11 function x = inverseKinematicsEuler(r des,a des,x init,epsilon)
12 % Input: desired endeffector position, desired endeffector ...
orientation (Eulerangles), initial guess for joint angles, ...
threshold for stoppingcriterion
13 % Output: joint angles with match desired endeffector position ...
and orientation
14 end

16
Solution 3.3

1 function a = jointToEulAng(x)
2 % Input: joint angles
3 % Output: corresponding euler angles with zyx sequence
4 R IE = jointToRotMat(x);
5 [yaw, pitch, roll] = dcm2angle(R IE','zyx');
6 a = [yaw;
7 pitch;
8 roll];
9 end
10
11 function X = xiMatrixEuler(a)
12 % Input: Euler angles with zyx sequence
13 % Output: 3x3 matrix mapping global rotational velocities to ...
Euler angle derivatives
14
15 ps = a(1);
16 th = a(2);
17
18 X = [cos(ps)*sin(th)/cos(th) sin(ps)*sin(th)/cos(th) 1;
19 sin(ps) cos(ps) 0;
20 cos(ps)/cos(th) sin(ps)/cos(th) 0];
21
22 end
23
24 function x = inverseKinematicsEuler(r des,a des,x init,epsilon)
25 % Input: desired endeffector position, desired endeffector ...
orientation (Eulerangles), initial guess for joint angles, ...
threshold for stoppingcriterion
26 % Output: joint angles which match desired endeffector position ...
and orientation
27
28 %% Setup
29 iterations = 0;
30 dth = inf(length(x init),1);
31 max iterations = 1000;
32
33 a des = a des(:);
34 r des = r des(:);
35 x init = x init(:);
36
37 % Initialize with initial guess
38 theta = x init;
39
40 % Iterate until terminating condition
41 while (norm(dth)>epsilon && iterations < max iterations)
42
43 dr = r des jointToPosition(theta);
44
45 a star = jointToEulAng(theta);
46
47 da = a desa star;
48
49 A = [jointToPosJac(theta);
50 xiMatrixEuler(a star)*jointToRotJac(theta)];
51
52 dth = pinv(A)*[dr; da];
53
54 % Update
55 theta = theta + dth;
56 iterations = iterations+1;
57
58 end
59
60 x = theta;

17
61
62 pos error = r des jointToPosition(x);
63 eul error = a des jointToEulAng(x);
64
65 fprintf('Inverse kinematics terminated after %d ...
iterations.\n',iterations);
66 fprintf('Position error: %e.\n',norm(pos error));
67 fprintf('Attitude error: %e.\n',norm(eul error));
68
69 end

Exercise 3.4
In this exercise you are going to implement a basic pose controller. The controller
will act only on the kinematic level, i.e. it will produce end-effector velocities as a
function of the current and desired end-effector pose.

References
[1] W. G. Breckenridge. Quaternions proposed standard conventions. Technical
report, NASA Jet Propulsion Laboratory.

18