Exercise 2: Dynamics of the ABB IRB 120

Dario Bellicoso, Remo Diethelm, Jemin Hwangbo, and Marco Hutter

October 20, 2015

Abstract

In this exercise you will learn how to calculate the equations of motionof an ABB robot arm and implement model-based control schemes. A MAT-LAB visualization of the robot arm is provided. You will have to imple-ment the tools that compute the dynamics of the arm in your MATLABscripts. Recalling what you learned in the last exercise, you will start byimplementing the Jacobians of the center of masses, and then implement themass matrix, the Coriolis and centrifugal terms, and the gravity terms. Inthe end, you will apply inverse-dynamics in order to have the robot followa desired path with the end-effector. The partially implemented MATLABscripts, as well as the visualizer, are available at https://bitbucket.org/ethz-asl-lr/robotdynamics exercise2. You can find some hints for MATLAB codeat the end of this document.

1 Dynamics

The robot arm and the dynamic properties are shown in figure 1. The kinematicand dynamic parameters are given and can be loaded using the provided MAT-LAB scripts. To initialize your workspace and to load the visualizer, run theinit exercise.m script.

1

Figure 1: ABB IRB 120 with coordinate systems and joints

Exercise 1.1

The provided MATLAB scripts generate gen coord.m, generate kin.m and gener-ate dyn.m can be used to load the dynamic parameters (mass, inertia, COM loca-tion in the joint frame for each link) of the ABB IRB 120 as well as the forwardkinematics. Using this information, find 1) the position and rotation Jacobians ofthe center of mass frames of each link, 2) position and rotation Jacobians of theend-effector, and 3) the time derivative of the position and rotation Jacobians ofthe end-effector by completing the implementation of generate jac.m.

Solution 1.1

1 % generate jacobians2 function jac = generate jac(gen cor, kin, dyn)3 % By calling:4 % jac = generate jac(gen cor, kin, dyn)5 % a struct 'jac' is returned that contains the translation and rotation6 % jacobians of the center of masses7

8 %% Setup9 phi = gen cor.phi;

10 dphi = gen cor.dphi;11

2

12 T Ik = kin.T Ik;13 R Ik = kin.R Ik;14 k omega hat k = kin.k omega hat k;15 I r Ie = kin.I r Ie;16

17 k r ks = dyn.k r ks;18

19 jac.I Jp s = cell(6,1);20 jac.I Jr = cell(6,1);21 jac.I Jpe = sym(zeros(3,6));22 jac.I Jre = sym(zeros(3,6));23

24

25 %% Compute link jacobians26

27 I Jp s = cell(6,1);28 I Jr = cell(6,1);29

30 for k=1:631

32 % create containers33 I Jp sk = sym(zeros(3,6));34 I Jrk = sym(zeros(3,6));35

36 % translational jacobian at the center of gravity s in frame I37 I r ks = [eye(3) zeros(3,1)]*T Ikk*[k r ksk;1];38 I Jp sk = jacobian(I r ks,phi);39

40 % rotational jacobian in frame I41 if k == 142 I Jr1(1:3,1) = R Ik1 * k omega hat k1;43 else44 % copy columns of k−1 jacobian45 I Jrk = I Jrk−1;46

47 % evaluate new column48 I Jrk(1:3,k) = R Ikk * k omega hat kk;49 end50

51 % simplify expressions52 I Jp sk = simplify(I Jp sk);53 I Jrk = simplify(I Jrk);54

55 end56

57 % Compute the end effector jacobians in frame I58 I Jpe = jacobian(I r Ie,phi);59 I Jre = I Jrk;60

61 % Compute the time derivative of the end effector Jacobians62 I dJpe = dAdt(I Jpe,phi,dphi);63 I dJre = dAdt(I Jre,phi,dphi);64

65 I Je = [I Jpe; I Jre];66 I dJe = [I dJpe; I dJre];67

68 % Generate function files from symbolic expressions69 fprintf('Generating Jacobian file... ');70 matlabFunction(I Je, 'vars', phi, 'file', 'I Je fun');71 fprintf('done!\n')72

73 fprintf('Generating dJe file... ');74 matlabFunction(I dJe, 'vars', phi,dphi, 'file', 'I dJe fun');75 fprintf('done!\n')76

77 % Store jacobians in output struct78 jac.I Jp s = I Jp s;

3

79 jac.I Jr = I Jr;80 jac.I Jpe = I Jpe;81 jac.I Jre = I Jre;82 jac.I dJpe = I dJpe;83 jac.I dJre = I dJre;84

85

86 end

1 function dA = dAdt( A, q, dq )2

3 % Initialize dA4 dA = sym(zeros(size(A)));5

6 % Evaluate time differentiation of A7 for i=1:size(A,1)8 for j=1:size(A,2)9 dA(i,j) = jacobian(A(i,j),q)*dq;

10 end11 end12

13 end

Exercise 1.2

Find the linear I~v4s and the rotational I~ω4s velocities of the COM of the 4th link in

inertial frame if ~ϕ = [π/6, π/6, π/6, π/6, π/6, π/6]T and ~ϕ = [π/6, π/6, π/6, π/6, π/6, π/6]T .

Solution 1.2

Recalling that in general it is:

I~v = IJP (φ) · φ

I~ω = IJR(φ) · φ(1)

using the results obtained from the first question one has:

I~v4s=

−0.00450.1513−0.2685

(2)

and

I~ω4s =

−0.2971.0380.070

(3)

Exercise 1.3

Using the methods you learned during the lecture, find the equations of motion (i.e.

M(~ϕ), ~b(~ϕ, ~ϕ), ~g(~ϕ)) by completing the implementation of the MATLAB scriptgenerate eom.m.

1 % generate equations of motion2 function eom = generate eom(gen cor, kin, dyn, jac)3

4 %% Setup5 phi = gen cor.phi;6 dphi = gen cor.dphi;7

8 T Ik = kin.T Ik;

4

9 R Ik = kin.R Ik;10

11 k I s = dyn.k I s;12 m = dyn.m;13 I g acc = dyn.I g acc;14 k r ks = dyn.k r ks;15

16 I Jp s = jac.I Jp s;17 I Jr = jac.I Jr;18

19 eom.M = sym(zeros(6,6));20 eom.C = sym(zeros(6,6));21 eom.g = sym(zeros(6,1));22 eom.n = sym(zeros(6,1));23 eom.U = sym(zeros(1,1));24

25

26 %% Compute mass matrix27 fprintf('Computing mass matrix M... ');28 M = sym(zeros(6,6));29 for k = 1:length(phi)30 M = M + mk*I Jp sk'*I Jp sk ...31 + I Jrk'*R Ikk*k I sk*R Ikk'*I Jrk;32 end33

34 % Use symmetry of B matrix to make computation time shorter35 fprintf('simplifying... ');36 for k = 1:length(phi)37 for h = k:length(phi)38 m kh = simplify(M(k,h));39 if h == k40 M(k,h) = m kh;41 else42 M(k,h) = m kh;43 M(h,k) = m kh;44 end45 end46 end47 fprintf('done!\n');48

49

50 %% Compute gravity terms51 fprintf('Computing gravity vector g... ');52 Upot = sym(0);53 for k=1:length(phi)54 Upot = Upot − mk*I g acc'*[eye(3) ...

zeros(3,1)]*T Ikk*[k r ksk;1];55 end56 g = jacobian(Upot, phi)';57 fprintf('simplifying... ');58 g = simplify(g);59 fprintf('done!\n');60

61

62 %% Compute nonlinear terms vector63 fprintf('Computing coriolis and centrifugal vector b and ...

simplifying... ');64 b = sym(zeros(6,1));65 for k=1:666 fprintf('b%i... ',k);67 dJp s = simplify(dAdt(jac.I Jp sk,gen cor.phi, gen cor.dphi));68 dJr s = simplify(dAdt(jac.I Jrk,gen cor.phi, gen cor.dphi));69 omega i = simplify(jac.I Jrk*gen cor.dphi);70

71 b = b + simplify(I Jp sk' * mk * dJp s * dphi) + ...72 simplify(I Jrk' * R Ikk * k I sk * R Ikk' * ...

dJr s * dphi) + ...

5

73 I Jrk' * cross( omega i , R Ikk * k I sk * ...R Ikk' * I Jrk * dphi );

74 end75 fprintf('done!\n');76

77

78 %% Compute energy79 fprintf('Computing energy U... ');80 Ukin = 0.5*dphi'*M*dphi;81 U = Ukin + Upot;82 fprintf('simplifying... ');83 U = simplify(U);84 fprintf('done!\n');85

86

87 %% Generate matlab functions88 fprintf('Generating eom scripts... ');89 fprintf('M... ');90 matlabFunction(M, 'vars', phi, 'file', 'M fun');91 fprintf('g... ');92 matlabFunction(g, 'vars', phi, 'file', 'g fun');93 fprintf('b... ');94 matlabFunction(b, 'vars', phi, dphi, 'file', 'b fun');95 fprintf('U... ');96 matlabFunction(U, 'vars', phi, dphi, 'file', 'U fun');97 fprintf('done!\n');98

99

100 %% Store the expressions101 eom.M = M;102 eom.g = g;103 eom.b = b;104 eom.U = U;105

106 end

2 Model-based control

In this section you will write three controllers that use the model of the robot arm toperform motion and force tracking tasks. Use the provided abb dynamics mdl.mdlSimulink model to simulate the dynamics of the arm. You can choose to implementyour controllers by writing the appropriate MATLAB scripts or by implementingthe control scheme in Simulink.In this section, a PD controller is a controller that generates a signal Y such as:

Y = ~kp (~xdes − ~x)− ~kd x, (4)

where denotes a row-wise multiplication. The interpretation of the control actionY depends on the controller and is described in each of the following subsections.

2.1 Joint space control

Exercise 2.1

Implement a PD controller in joint space that counteracts the effect of the gravity.This controller can be written as ~τ = C(~ϕ, ~ϕ, ~ϕdes). The PD controller here outputsthe desired joint torque directly.

1

2 function [ tau ] = control pd g( phi des, phi, phi dot )

6

3

4 %CONTROLLER1 Joint space controller with gravity compensation5

6 % Gains7 % Here the controller response is mainly inertia dependent8 % so the gains have to be tuned joint−wise9 kp = 0.01 * [5000,3000,5,1,0.5,0.01]';

10 kd = 0.05 * [3000,2000,5,1,0.5,0.01]';11

12 % Joint wise feedback13 tau = kp .* (phi des − phi) ...14 − kd .* phi dot ...15 + g fun(phi);16

17 end

Exercise 2.2

Does the end-effector position converge to the desired position? What would happenif you neglect the gravitational compensation?

Solution 2.2

Recall that the equations of motion are expressed by:

M(~ϕ) +~b(~ϕ, ~ϕ) + ~g(~ϕ) = ~τ (5)

A PD control that compensates for gravity can be chosen as:

~τ = ~kp (~ϕdes − ~ϕ)− ~kd ~ϕ+ ~g(~ϕ) (6)

Under this control action, at steady state (i.e. ~ϕ ≡ ~0, ~ϕ ≡ ~0) one has:

~kp (~ϕdes − ~ϕ) = 0 (7)

Hence, by choosing kpi > 0∀i, one has:

~ϕdes − ~ϕ = 0 =⇒ ~ϕ = ~ϕdes (8)

By exactly compensating gravity, we have proven that the desired joint position~ϕdes is an asymptotically stable equilibrium point.Consider now the realistic case in which the model of the arm is not perfectly known.The controller will be:

~τ = ~kp (~ϕdes − ~ϕ)− ~kd ~ϕ+ ~g(~ϕ) (9)

where ~g(~ϕ) ∈ R6 is the available estimate of the effect of gravity on each joint. Theequations of motion at steady state are now:

~kp (~ϕdes − ~ϕ) + ~g(~ϕ) = ~g(~ϕ) (10)

The steady state is now:

(~ϕdes − ~ϕ) = ~k−1p (~g(~ϕ)− ~g(~ϕ)) (11)

where ~k−1p is the 6 × 1 vector whose elements are the inverse of those of ~kp. This

shows in the realistic case of imperfect cancellation of gravity the system exhibits asteady state error. This can be reduced by increasing the gains kpi, which in turnleads to a (potentially undesirable) more aggressive control action.

7

2.2 Inverse dynamics control

Exercise 2.3

Implement a PD controller in the operational space that fully counteracts all thenon-linearity of the system. This controller can be written as ~τ = C(~ϕ, ~ϕ, ~xE,des),where ~xE,des is the desired end effector position in the operational space. ThePD controller here outputs the desired acceleration in operational space. Use theprovided model function or Simulink block to test your controller.

Solution 2.3

1

2 function [ tau ] = control inv dyn( x des, phi, phi dot )3

4 %CONTROLLER2 Inverse dynamic controller with a pd control from ...position to acceleration

5

6 % Gains7 kp = 10 * [1,1,1,0.5,0.5,0.5]';8 kd = 10 * [1,1,1,1,1,1]';9

10 % Find jacobians, positions and orientation11 J e = I Je fun(phi);12 J e dot = I dJe fun(phi, phi dot);13 TIe = T IE fun(phi);14 r Ie = TIe(1:3, 4);15 RIe = TIe(1:3, 1:3);16 qIe = rotMatToQuat(RIe);17

18 % Define error orientation in Angle & Axis19 qerror = quatMult(invertQuat(qIe), x des(4:7));20 ErrorAxisInEframe = qerror(2:4) ./ norm(qerror(2:4));21 ErrorAxisInInertialFrame = RIe * ErrorAxisInEframe;22 ErrorAngle = 2 * acos(qerror(1));23

24 % PD law, the orientation feedback is a torque around error ...rotation axis proportional to the error angle.

25 x dot = J e * phi dot;26 x ddot des = kp .* ([x des(1:3) − r Ie; −ErrorAngle * ...

ErrorAxisInInertialFrame]) − kd .* x dot;27 phi dot dot = J e \ x ddot des − J e \ J e dot * phi dot;28

29 %Inverse dynamics30 tau = M fun(phi) * phi dot dot + b fun(phi, phi dot) + g fun(phi);31

32 end

Exercise 2.4

The simulator inputs a constant desired position and zero initial velocity to the con-troller. How would the end-effector trajectory look like (assuming perfect knowledgeof the model)?

Solution 2.4

The end effector position trajectory forms a line. The end effector orientation tra-jectory has a constant axis of rotation (i.e. the quaternion trajectory is a geodesic).

8

Figure 2: Robot arm cleaning a window

2.3 Operational space control

Exercise 2.5

As shown in figure 2, there is a window at x = 0.1. Write a controller that wipes thewindow. This controller applies a constant force on the wall in x-axis and followsa trajectory defined on y,z-plane. It can be written as ~τ = C(~ϕ, ~ϕ, ~xE,des,Fx). PDcontroller here outputs the desired acceleration in the operational space.

1

2 function [ tau ] = control op space( x des, phi, phi dot, Fx )3

4 %CONTROLLER3 operational space controller5

6 % Gains7 kp = 20 * [0,1,1,0.01,0.01,0.01]';8 kd = 40.0 * [1,1,1,1,1,1]';9

10 % Endeffector force11 F e = [Fx, 0, 0, 0, 0, 0]';12

13 % Find jacobians, positions and orientation

9

14 J e = I Je fun(phi);15 J e dot = I dJe fun(phi, phi dot);16 TIe = T IE fun(phi);17 r Ie = TIe(1:3, 4);18 RIe = TIe(1:3, 1:3);19 qIe = rotMatToQuat(RIe);20

21 % Project M b g to operational space22 lambda = (J e') \ M fun(phi) / J e;23 mu = (J e') \ b fun(phi, phi dot) − lambda * J e dot * phi dot;24 p = (J e') \ g fun(phi);25

26 % Define error orientation in Angle & Axis27 qerror = quatMult(invertQuat(qIe), x des(4:7));28 ErrorAxisInEframe = qerror(2:4) ./ norm(qerror(2:4));29 ErrorAxisInInertialFrame = RIe * ErrorAxisInEframe;30 ErrorAngle = 2 * acos(qerror(1));31

32 % PD law, the orientation feedback is a torque around error ...rotation axis proportional to the error angle.

33 x dot = J e * phi dot;34 F e des = lambda * (kp .* ([x des(1:3) − r Ie; −ErrorAngle * ...

ErrorAxisInInertialFrame]) − kd .* x dot) ...35 + F e + mu + p;36

37 % Map Force back to the joint space38 tau = J e' * F e des;39

40 end

10

3 Some Matlab Tools

3.1 Generalized Coordinates

We use generalized coordinates ~ϕ that can contain, in the case of free floating bodies,un-actuated base coordinates ~ϕb in addition to the joint coordinates ~ϕr:

~ϕ =

(~ϕb

~ϕr

)(12)

1 % define generalized coordinates2 syms x q1 q2 real3 phi = [x,q1,q2]';4 % define generalized velocities5 syms Dx Dq1 Dq2 real6 dphi = [Dx,Dq1,Dq2]';

3.2 Position Vector

A (position) vector from point O to P as a function of generalized coordinates ~ϕexpressed in frame B:

B~rOP =B ~rOP (~ϕ) (13)

1 % define certain parameters2 syms l real3 % position vector4 r = [l*sin(q1),l*cos(q1),0];

3.3 Velocity (in Moved Systems)

The velocity is given through differentiation, whereby special attention is requiredwhen differentiating in a moving (with respect to inertial frame I) coordinatessystem B:

I~r → I~r =dI~r

dt(14)

B~r → B~r =dB~r

dt+B ~ωIB ×B ~r (15)

In this document, O refers to the origin of a frame, I indicates the inertial frameand B a body fixed frame (which can be moved).

In the case of rigid body kinematics, the body angular velocity is often indicatedby ~Ω

~Ω = I~ωIB (16)

1 % derivation of position vectors expressed in inertia frame2 dr = dMATdt(r,q,dq);

using the function

1 function [ dA ] = dMATdt( A, q, dq )2 dA = sym(zeros(size(A)));

11

3 for i=1:size(A,1)4 for j=1:size(A,2)5 dA(i,j) = jacobian(A(i,j),q)*dq;6 end7 end8 end

3.4 Rotation

Ix

Iy

Iz

Bx

By

Bz

j

O

Figure 3: Rotated coordinate system B around z axis

The rotation matrix RIB rotates a vector B~r expressed in frame B to frame I:

I~r = RIBB~r (17)

B~r = RBII~r RBI = RTIB (18)

C~r = RCII~r RCI = RCBRBI (19)

In this tool we use the x-y-z convention.The symbolic rotational matrices can be generated using the following function

1 function R IB = eulerToRotMat R IB(al,be,ga)2 R IB = sym(zeros(3));3 R IB(1,1) = cos(be)*cos(ga);4 R IB(2,1) = cos(ga)*sin(al)*sin(be) + cos(al)*sin(ga);5 R IB(3,1) = −(cos(al)*cos(ga)*sin(be)) +sin(al)*sin(ga);6 R IB(1,2) = −(cos(be)*sin(ga));7 R IB(2,2) = cos(al)*cos(ga) − sin(al)*sin(be)*sin(ga);8 R IB(3,2) = cos(ga)*sin(al) + cos(al)*sin(be)*sin(ga);9 R IB(1,3) = sin(be);

10 R IB(2,3) = −(cos(be)*sin(al));11 R IB(3,3) = cos(al)*cos(be);12 end

3.5 Angular Velocity

Given a rotation matrix RIB , the corresponding rotation speed I~Ω of a rigid body

with body fixed frame B is:

IΩ = IωIB = RIBRTIB (20)

Ω =

0 −Ωz Ωy

Ωz 0 −Ωx

−Ωy Ωx 0

, unskewskew

Ω =

Ωx

Ωy

Ωz

(21)

12

1 % rotation matrix around z with vector phi2 RIB = eulerToRotMat R IB(0,0,phi);3 % differentiate rotation matrix4 dRIB = dMATdt(dRIB,q,dq);5 % generate rotation speed and unskew it6 I Omega = unskew(simplify(dAIB*AIB'));

with the skew and unskew function

1 % skew function2 function A=skew(a)3 A = [0 −a(3) a(2);4 a(3) 0 −a(1);5 −a(2) a(1) 0];6

7 % unskew function8 function a=unskew(A)9 a = [A(3,2); A(1,3); A(2,1)];

3.6 Jacobian

The Jacobian J is given through:

J (~ϕ) =∂~r (~ϕ)

∂~ϕ=

∂~r1∂~ϕ1

∂~r1∂~ϕ2

. . . ∂~r1∂~ϕn

∂~r2∂~ϕ1

∂~r2∂~ϕ2

. . . ∂~r2∂~ϕn

......

. . ....

∂~rm∂~ϕ1

∂~rm∂~ϕ2

. . . ∂~rm∂~ϕn

, ~ϕ ∈ <n×1, ~r ∈ <m×1 (22)

Jacobians are used to map generalized velocities ~ϕ to Cartesian velocities ~r:

~r = J ~ϕ (23)

and in its dual problem to map Cartesian forces F to generalized forces τ :

τ = JTF (24)

We differ between translational Jacobians JP = ∂~rP (~ϕ)∂~ϕ , which correspond to a

specific point P and rotational Jacobians JR = ∂~Ω(~ϕ)

∂ ~ϕthat are identical for all

points of one single rigid body.

1 % get jacobian from position vector2 J = jacobian(r,q);3 % get jacobian from rotation speed4 Jr = jacobian(Omega,dq);

13