Exercise 1: Kinematics of the ABB IRB 120

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 isprovided. You will have to implement the tools to compute the forward andinverse kinematics in your Matlab script. During this you will exercise how towork with different representations of the end-effector orientation and how tocheck whether your implementations are correct. In the end, you will applyinverse-kinematics in order to have the robot follow a desired path with theend-effector.

1 Introduction

The following exercise is for the ABB IRB 120 depicted in figure 2. It is a 6-linkrobotic manipulator with a fixed base. During the exercise you will implementa lot of different Matlab functions - test them carefully since the following tasksare often dependent on them. To help you with this, we have provided the scriptprototypes at bitbucket.org/ethz-asl-lr/robotdynamics_exercise1 togetherwith a visualizer of the manipulator.

1

bitbucket.org/ethz-asl-lr/robotdynamics_exercise1

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 systemattached to the end-effector (coordinate system P6 in figure 2).

Exercise 2.1

Define the generalized ~θ coordinates for the ABB IRB120. Please remember thatgeneralized coordinates should be complete (the configuration of the robot shouldbe fully specified by the generalized coordinates) and independent (no coordinateis 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 singlejoint angles is also a valid solution.

~θ = (θ1, . . . , θ6)T ∈ R6×1 (1)

Exercise 2.2

Find the homogenous transformations matrices Tk−1,k(θk), ∀k = 1, . . . , 6. Addi-tionally, find the constant homogeneous transformations between the inertial frameand frame 0 (TI0) and between frame 6 and the end-effector frame (T6E). Pleaseimplement the following functions:

1 function TI0 = getTransformI0()2 % Input: void3 % Output: homogeneous transformation Matrix from the inertial ...

frame 0 to frame I. T I04 end5

2

6 function T01 = jointToTransform01(x)7 % Input: joint angles8 % Output: homogeneous transformation Matrix from frame 1 to frame ...

0. T 019 end

10


1. T 1214 end15


2. T 2319 end20


3. T 3424 end25


4. T 4529 end30


5. T 5634 end35

36 function T6E = getTransform6E()37 % Input: void38 % Output: homogeneous transformation Matrix from the end−effector ...

frame E to frame 6. T 6E39 end

Solution 2.2

Remember that a homogeneous transformation matrix is expressed in the form:

Thk(θk) =

[Rhk(θk) h~rhk(θk)~01×3 1

](2)

For the ABB IRB 120, each Thk(θk) is composed by an elementary rotation a singlejoint axis and a translation defined by the manipulator kinematic parameters. Bydefining the elementary rotations matrices about each axis as:

Rz(α) =

cos(α) − sin(α) 0sin(α) cos(α) 0

0 0 1

(3)

Ry(β) =

cos(β) 0 sin(β)0 1 0

− sin(β) 0 cos(β)

(4)

Rx(γ) =

1 0 00 cos(γ) − sin(γ)0 sin(γ) cos(γ)

(5)

one can write:

3

T01(θ1) =

[Rz(θ1) 0~r01(θ1)~01×3 1

](6)

T12(θ2) =

[Ry(θ2) 1~r12(θ2)~01×3 1

](7)

T23(θ3) =

[Ry(θ3) 2~r23(θ3)~01×3 1

](8)

T34(θ4) =

[Rx(θ4) 3~r34(θ4)~01×3 1

](9)

T45(θ5) =

[Ry(θ5) 4~r45(θ5)~01×3 1

](10)

T56(θ6) =

[Rx(θ6) 5~r56(θ6)~01×3 1

](11)

Finally, the constant homogeneous transformations TIE and T6E are simply theidentity matrix I4×4.

1 function TI0 = getTransformI0()2 % Input: void3 % Output: homogeneous transformation Matrix from the inertial ...

frame 0 to frame I. T I04 TI0 = eye(4);5 end6

7 function T01 = jointToTransform01(x)8 % Input: joint angle9 % Output: homogeneous transformation Matrix from frame 1 to frame ...

0. T 0110 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 end15


1. T 1219 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 end24


2. T 2328 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 end33


3. T 3437 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 end42


4. T 4546 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 end51


6. T 5655 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 end60

61 function T6E = getTransform6E()62 % Input: void63 % Output: homogeneous transformation Matrix from the end−effector ...

frame E to frame 6. T 6E64 T6E = eye(4);65 end

Exercise 2.3

Find the end-effector position I~rIE = I~rIE(~θ). Please implement the followingfunction:

1 function r = jointToPosition(x)2 % Input: joint angles3 % Output: position of end−effector w.r.t. inertial frame. I r IE4 end

Solution 2.3

The end-effector position is given by the direct kinematics, represented in matrixform by the homogeneous transformation TIE(~θ), which can be found by successiveconcatenation of coordinate frame transformations.

TIE(~θ) = TI0 ·

(6∏

k=1

Tk−1,k

)·T6E =

[RIE I~rIE

(~θ)

~01×3 1

](12)

The end-effector position can then be found by selecting the fourth column ofTIE(~θ).

[~r(~θ)

1

]= TIE(~θ) ·

0001

(13)

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

5

3 % Output: position of end−effector w.r.t. inertial frame. I r IE4

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

What is the end-effector position for ~θ =

π/6π/6π/6π/6π/6π/6

? Use matlab to display it.

Solution 2.4

From the direct kinematics equations found earlier, it is:

I~rIE = I~rIE

(~θ)

=

0.29480.19100.2277

. (14)

Exercise 2.5

Find the analytical expression of the end-effector linear velocity I~vIE = I~vIE

(~θ)

=

dI~rIE

(~θ)

dt.

Solution 2.5

P

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 hasa linear velocity 0v01 and angular velocity 0ω1. Consider a point P that is fixed in

6

frame 1. The linear velocity 0vP02 of point P with respect to the fixed frame 0 is

given by:

0vP02 = 0v01 + 0r12 + 0ω1 × 0r12. (15)

If point P is fixed in frame 1, it is 0r12 = 0. With this result in mind, consider nowa planar two link robot arm with two revolute joints. The coordinate frames arechosen as in Fig.2. Reasoning as in Eq.(??), the linear velocity at the end of thekinematic chain can be found by propagating the velocity contributions from thefixed frame 0. Hence, one has:

0v01 = 0v0 + 0ω0 × 0r01

0v02 = 0v01 + 0ω1 × 0r12

0v0E = 0v02 + 0ω2 × 0r2E

(16)

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, 0v0 = 0),the end-effector linear velocity is given by:

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

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

I~vIE = Iω1 × Ir12 + Iω2 × Ir23 + · · ·+ Iω5 × Ir56 + IωE × Ir6E

= I~v01 + I~v12 + · · ·+ I~v56 + I~v6E(18)

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 angles3 % Output: rotation Matrix of the end−effector. R IE4 end

7

Solution 2.6

From the structure of the direct kinematics equations found earlier, it follows thatthe end-effector rotation matrix is obtained by extracting the first three rows andthe first three columns from TIE(~θ). This operation can be compactly written inmatrix form:

RIE

(~θ)

=[~I3×3

~03×1

]·TIE(~θ) ·

[~I3×3

~01×3

]. (19)

1 function R = jointToRotMat(x)2 % Input: joint angles3 % Output: rotation Matrix of the end−effector. R EI4

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 qIE = qIE

(~θ)

.

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 andback to quaternions.

• The quaternion multiplication q⊗p (unfortunately the quaternion implemen-tation in the aerospace toolbox of Matlab uses Hamilton convention, henceuse the definition from the lecture slides, where the JPL [1] convention hasbeen 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. Theeasiest way is to transform the quaternion to the corresponding rotation ma-trix (by using the function from above) and then multiply the matrix withthe vector to be rotated.

Also check that your two representations for the end-effector orientation match witheach other. In total you should write the following five functions:

1 function q = jointToQuat(x)2 % Input: joint angles3 % Output: quaternion representing the orientation of the ...

end−effector. q EI4 end5

6 function R = quatToRotMat(q)7 % Input: quaternion [w x y z]8 % Output: corresponding rotation matrix

8

9 end10

11 function q = rotMatToQuat(R)12 % Input: rotation matrix13 % Output: corresponding quaternion [w x y z]14 end15

16 function q AC = quatMult(q AB,q BC)17 % Input: two quaternions to be multiplied18 % Output: output of the multiplication19 end20

21 function B r = rotVecWithQuat(q BA,A r)22 % Input: the orientation quaternion and the coordinate of the ...

vector to be mapped23 % Output: the coordinates of the vector in the target frame24 end

Solution 2.7

RIE

(~θ)

= RI0 ·

(6∏

k=1

Rk−1,k

)·R6E (20)

q(~θ)

=

√

(1 + T ) /4(R23 −R32) /(4 · q1)(R31 −R13) /(4 · q1)(R12 −R21) /(4 · q1)

(21)

where T = Trace(RIE).

1 function q = jointToQuat(x)2 % Input: joint angles3 % Output: quaternion representing the orientation of the ...

end−effector. q EI4

5 R = jointToRotMat(x);6 q = rotMatToQuat(R);7 end8

9 function R = quatToRotMat(q)10 % Input: quaternion [w x y z]11 % Output: corresponding rotation matrix12

13 q = q(:);14 R = (2*q(1)ˆ2−1)*eye(3) − 2*q(1)*skewMatrix(q(2:end)) + ...

2*q(2:end)*q(2:end)';15 end16

17 function q = rotMatToQuat(R)18 % Input: rotation matrix19 % Output: corresponding quaternion [w x y z]20

21 % JPL22 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 end28

29 function q AC = quatMult(q AB,q BC)30 % Input: two quaternions to be multiplied31 % Output: output of the multiplication32

9

33 q = q AB;34 p = q BC;35

36 % JPL37 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 end42

43 function B r = rotVecWithQuat(q BA,A r)44 % Input: the orientation quaternion and the coordinate of the ...

vector to be mapped45 % Output: the coordinates of the vector in the target frame46

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 = I~ωIE

(~θ)

.

Solution 2.8

The end-effector rotational velocity I~ωIE is obtained by summing the single jointvelocity contributions:

I~ωIE = I~ωI0 + I~ω01 + I~ω12 + · · ·+ 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 linearand rotational velocity derived above in exercise 2. The Jacobians should dependon 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 angles3 % Output: Jacobian of the end−effector position4 end5

6 function J R = jointToRotJac(x)7 % Input: joint angles8 % Output: Jacobian of the end−effector orientation9 end

Solution 3.1

The translation and rotation Jacobians can be evaluated starting from the resultsthat 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 × Ir12 + Iω2 × Ir23 + · · ·+ Iω5 × Ir56 + IωE × Ir6E

= Iω1 × (IrI2 − IrI1) + Iω2 × (IrI3 − IrI2) + · · ·+ Iω5 × (IrI6 − IrI5)

= (Iω0 + Iω01)× (IrI2 − IrI1)

+ (Iω1 + Iω12)× (IrI3 − IrI2)

+ . . .

+ (Iω5 + Iω56)× (IrIE − IrI6)

(25)

At this point it is worth noticing that it is Iωk−1,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ωk−1 + Iωk−1,k, (26)

one has:

I~vIE = (Iω0 + Iω01)× (IrI2 − IrI1)

+ (Iω1 + Iω12)× (IrI3 − IrI2)

+ . . .

+ (Iω5 + Iω56)× (IrIE − IrI6)

= (I ω1θ1)× (IrI2 − IrI1)

+ (I ω1θ1 + I ω2θ2)× (IrI3 − IrI2)

+ . . .

+ (I ω1θ1 + · · ·+ I ω6θ6)× (IrIE − IrI6)

(27)

11

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

I~vIE = I ω1θ1 × (IrIE − IrI1)

+ I ω2θ2 × (IrIE − IrI2)

+ . . .

+ I ω6θ6 × (IrIE − IrI6)

, (28)

which, rewritten in matrix from, gives:

I~vIE =[I ω1 × (IrIE − IrI1) . . . I ω6 × (IrIE − IrI6)

] θ1. . .θ6

= JP (~θ)~θ,

(29)

where JP (~θ) is the translation Jacobian matrix that projects a vector from the jointvelocity 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 ~ω1θ1 + I ~ω2θ2 + · · ·+ I ~ω6θ6

=[I ~ω1 I ~ω2 . . . I ~ω6

]· ~θ

= JR(~θ)~θ

, (30)

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

RIE(~θ) = S(I~ωIE) ·RIE(~θ), (31)

where S(I~ωIE) is the skew matrix operator defined as:

S(~α) =

0 −αz αy

αz 0 −αx

−αy αx 0

. (32)

Using the chain rule, one can also write:

R(~θ) =∂R(~θ)

∂~θ· ~θ (33)

Combining the last two equations and using the orthonormality property of rotationmatrices, one has:

S(I~ωIE) = RIE(~θ) ·RIE(~θ)T . (34)

The end-effector velocity I~ωIE can be easily extracted from S(I~ωIE). The rotationalJacobian can now be evaluated as:

JR(~θ) =∂I~ωIE

∂θ(35)

12

1 function J P = jointToPosJac(x)2 % Input: joint angles3 % Output: Jacobian of the end−effector position4 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 end50

51 function J R = jointToRotJac(x)52 % Input: joint angles53 % Output: Jacobian of the end−effector orientation54 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∗ =[0.5649 0 0.5509

]Twhile its orientation is kept as identity. To this end you

will have to implement an iterative inverse kinematics algorithm in Matlab. Thealgorithm 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 ∆qi = q∗ − q(~θi), with q∗ = (1, 0, 0, 0).

3. Construct the following system of linear equations and solve for ∆~θi.(∆~ri∆qi

)=

[JP (~θi)

ξ(q(~θi))JR(~θi)

]∆~θi (36)

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+1and continue with item 2.

Please implement your solution in the following function. You will also need toimplement the function ξ(q(~θ)) which maps the rotational velocity to the quaternionderivative.

1 function X = xiMatrix(q)2 % Input: quaternion3 % Output: 4x3 matrix mapping local rotational velocities to ...

quaternion derivatives4 end5

6 function x = inverseKinematics(r des,q des,x init,epsilon)7 % Input: desired end−effector position, desired end−effector ...

orientation (quaternion), initial guess for joint angles, ...threshold for stopping−criterion

8 % Output: joint angles with match desired end−effector position ...and orientation

9 end

14

Solution 3.2

Recall that the mapping between the time derivative of the quaternion qEI and thelocal angular velocity E~ωIE is given by:

qEI =1

2Ξ(qEI(~θ)) · E~ωIE , (37)

where

Ξ(qEI(~θ)) =

[−qEI

qEI,0I3×3 + qxBI

](38)

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: quaternion3 % Output: 4x3 matrix mapping local rotational velocities to ...

quaternion derivatives4

5 % Make sure that q is a column6 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 end−effector position, desired end−effector ...

orientation (quaternion), initial guess for joint angles, ...threshold for stopping−criterion

13 % Output: joint angles which match desired end−effector position ...and orientation

14

15 %% Setup16 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 guess27 theta = x init;28

29 % Iterate until terminating condition30 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 EI−q 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 % Update45 theta = theta + dth;46 iterations = iterations+1;

15

47

48 end49

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 end60

61 function q inv = invertQuat(q)62 % Input: a unit quaternion63 % Output: the inverse of the input quaternion64 q = q(:);65 q inv = [q(1); −q(2:end)];66 q inv = q inv/norm(q inv);67 end68

69 function S = skewMatrix(x)70 % Input: a vector in R371 % 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 fullrotation matrix or quaternions. This has the advantage that subsequent relativeorientations can just be concatenated by multiplying the matrices or quaternions to-gether. Now, in order to get a more compact and human-readable representation ofthe 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 rotationJacobian. You should implement the following functions:

1 function a = jointToEulAng(x)2 % Input: joint angles3 % Output: corresponding euler angles with z−y−x sequence4 end5

6 function X = xiMatrixEuler(a)7 % Input: z−y−x Euler angles8 % Output: 3x3 matrix mapping global rotational velocities to ...

Euler angle derivatives9 end

10

11 function x = inverseKinematicsEuler(r des,a des,x init,epsilon)12 % Input: desired end−effector position, desired end−effector ...

orientation (Euler−angles), initial guess for joint angles, ...threshold for stopping−criterion

13 % Output: joint angles with match desired end−effector position ...and orientation

14 end

16

Solution 3.3

1 function a = jointToEulAng(x)2 % Input: joint angles3 % Output: corresponding euler angles with z−y−x sequence4 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 z−y−x sequence13 % Output: 3x3 matrix mapping global rotational velocities to ...

Euler angle derivatives14

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 end23

24 function x = inverseKinematicsEuler(r des,a des,x init,epsilon)25 % Input: desired end−effector position, desired end−effector ...

orientation (Euler−angles), initial guess for joint angles, ...threshold for stopping−criterion

26 % Output: joint angles which match desired end−effector position ...and orientation

27

28 %% Setup29 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 guess38 theta = x init;39

40 % Iterate until terminating condition41 while (norm(dth)>epsilon && iterations < max iterations)42

43 dr = r des − jointToPosition(theta);44

45 a star = jointToEulAng(theta);46

47 da = a des−a star;48

49 A = [jointToPosJac(theta);50 xiMatrixEuler(a star)*jointToRotJac(theta)];51

52 dth = pinv(A)*[dr; da];53

54 % Update55 theta = theta + dth;56 iterations = iterations+1;57

58 end59

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 controllerwill act only on the kinematic level, i.e. it will produce end-effector velocities as afunction of the current and desired end-effector pose.

References

[1] W. G. Breckenridge. Quaternions – proposed standard conventions. Technicalreport, NASA Jet Propulsion Laboratory.

18

Exercise 1: Kinematics of the ABB IRB 120

Documents

Transcript of Exercise 1: Kinematics of the ABB IRB 120