Exercise 2: Dynamics of the ABB IRB 120

Transcription

1 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 motion of an ABB robot arm and implement model-based control schemes. A MAT- LAB visualization of the robot arm is provided. You will have to implement the tools that compute the dynamics of the arm in your MATLAB scripts. Recalling what you learned in the last exercise, you will start by implementing the Jacobians of the center of masses, and then implement the mass matrix, the Coriolis and centrifugal terms, and the gravity terms. In the end, you will apply inverse-dynamics in order to have the robot follow a desired path with the end-effector. The partially implemented MATLAB scripts, as well as the visualizer, are available at exercise2. You can find some hints for MATLAB code at the end of this document. 1 Dynamics The robot arm and the dynamic properties are shown in figure 1. The kinematic and dynamic parameters are given and can be loaded using the provided MAT- LAB scripts. To initialize your workspace and to load the visualizer, run the init exercise.m script. 1

2 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 generate dyn.m can be used to load the dynamic parameters (mass, inertia, COM location in the joint frame for each link) of the ABB IRB 120 as well as the forward kinematics. Using this information, find 1) the position and rotation Jacobians of the center of mass frames of each link, 2) position and rotation Jacobians of the end-effector, and 3) the time derivative of the position and rotation Jacobians of the end-effector by completing the implementation of generate jac.m. Solution % generate jacobians 2 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 rotation 6 % jacobians of the center of masses 7 8 %% Setup 9 phi = gen cor.phi; 10 dphi = gen cor.dphi; 11 2

3 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; k r ks = dyn.k r ks; 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)); %% Compute link jacobians I Jp s = cell(6,1); 28 I Jr = cell(6,1); for k=1: % create containers 33 I Jp s{k} = sym(zeros(3,6)); 34 I Jr{k} = sym(zeros(3,6)); % translational jacobian at the center of gravity s in frame I 37 I r ks = [eye(3) zeros(3,1)]*t Ik{k}*[k r ks{k};1]; 38 I Jp s{k} = jacobian(i r ks,phi); % rotational jacobian in frame I 41 if k == 1 42 I Jr{1}(1:3,1) = R Ik{1} * k omega hat k{1}; 43 else 44 % copy columns of k 1 jacobian 45 I Jr{k} = I Jr{k 1}; % evaluate new column 48 I Jr{k}(1:3,k) = R Ik{k} * k omega hat k{k}; 49 end % simplify expressions 52 I Jp s{k} = simplify(i Jp s{k}); 53 I Jr{k} = simplify(i Jr{k}); end % Compute the end effector jacobians in frame I 58 I Jpe = jacobian(i r Ie,phi); 59 I Jre = I Jr{k}; % Compute the time derivative of the end effector Jacobians 62 I djpe = dadt(i Jpe,phi,dphi); 63 I djre = dadt(i Jre,phi,dphi); I Je = [I Jpe; I Jre]; 66 I dje = [I djpe; I djre]; % Generate function files from symbolic expressions 69 fprintf('generating Jacobian file... '); 70 matlabfunction(i Je, 'vars', {phi}, 'file', 'I Je fun'); 71 fprintf('done!\n') fprintf('generating dje file... '); 74 matlabfunction(i dje, 'vars', {phi,dphi}, 'file', 'I dje fun'); 75 fprintf('done!\n') % Store jacobians in output struct 78 jac.i Jp s = I Jp s; 3

4 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; end 1 function da = dadt( A, q, dq ) 2 3 % Initialize da 4 da = sym(zeros(size(a))); 5 6 % Evaluate time differentiation of A 7 for i=1:size(a,1) 8 for j=1:size(a,2) 9 da(i,j) = jacobian(a(i,j),q)*dq; 10 end 11 end end Exercise 1.2 Find the linear I v 4s and the rotational I ω 4s velocities of the COM of the 4 th 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 = I J P (φ) φ I ω = I J R (φ) φ using the results obtained from the first question one has: I v 4s = (2) and (1) I ω 4s = (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 script generate eom.m. 1 % generate equations of motion 2 function eom = generate eom(gen cor, kin, dyn, jac) 3 4 %% Setup 5 phi = gen cor.phi; 6 dphi = gen cor.dphi; 7 8 T Ik = kin.t Ik; 4

5 9 R Ik = kin.r Ik; 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; I Jp s = jac.i Jp s; 17 I Jr = jac.i Jr; 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)); %% Compute mass matrix 27 fprintf('computing mass matrix M... '); 28 M = sym(zeros(6,6)); 29 for k = 1:length(phi) 30 M = M + m{k}*i Jp s{k}'*i Jp s{k} I Jr{k}'*R Ik{k}*k I s{k}*r Ik{k}'*I Jr{k}; 32 end % Use symmetry of B matrix to make computation time shorter 35 fprintf('simplifying... '); 36 for k = 1:length(phi) 37 for h = k:length(phi) 38 m kh = simplify(m(k,h)); 39 if h == k 40 M(k,h) = m kh; 41 else 42 M(k,h) = m kh; 43 M(h,k) = m kh; 44 end 45 end 46 end 47 fprintf('done!\n'); %% Compute gravity terms 51 fprintf('computing gravity vector g... '); 52 Upot = sym(0); 53 for k=1:length(phi) 54 Upot = Upot m{k}*i g acc'*[eye(3)... zeros(3,1)]*t Ik{k}*[k r ks{k};1]; 55 end 56 g = jacobian(upot, phi)'; 57 fprintf('simplifying... '); 58 g = simplify(g); 59 fprintf('done!\n'); %% Compute nonlinear terms vector 63 fprintf('computing coriolis and centrifugal vector b and... simplifying... '); 64 b = sym(zeros(6,1)); 65 for k=1:6 66 fprintf('b%i... ',k); 67 djp s = simplify(dadt(jac.i Jp s{k},gen cor.phi, gen cor.dphi)); 68 djr s = simplify(dadt(jac.i Jr{k},gen cor.phi, gen cor.dphi)); 69 omega i = simplify(jac.i Jr{k}*gen cor.dphi); b = b + simplify(i Jp s{k}' * m{k} * djp s * dphi) simplify(i Jr{k}' * R Ik{k} * k I s{k} * R Ik{k}' *... djr s * dphi)

6 73 I Jr{k}' * cross( omega i, R Ik{k} * k I s{k} *... R Ik{k}' * I Jr{k} * dphi ); 74 end 75 fprintf('done!\n'); %% Compute energy 79 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'); %% Generate matlab functions 88 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'); %% Store the expressions 101 eom.m = M; 102 eom.g = g; 103 eom.b = b; 104 eom.u = U; end 2 Model-based control In this section you will write three controllers that use the model of the robot arm to perform motion and force tracking tasks. Use the provided abb dynamics mdl.mdl Simulink model to simulate the dynamics of the arm. You can choose to implement your controllers by writing the appropriate MATLAB scripts or by implementing the control scheme in Simulink. In this section, a PD controller is a controller that generates a signal Y such as: Y = k p ( x des x) k d ẋ, (4) where denotes a row-wise multiplication. The interpretation of the control action Y 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 outputs the desired joint torque directly. 1 2 function [ tau ] = control pd g( phi des, phi, phi dot ) 6

7 3 4 %CONTROLLER1 Joint space controller with gravity compensation 5 6 % Gains 7 % Here the controller response is mainly inertia dependent 8 % so the gains have to be tuned joint wise 9 kp = 0.01 * [5000,3000,5,1,0.5,0.01]'; 10 kd = 0.05 * [3000,2000,5,1,0.5,0.01]'; % Joint wise feedback 13 tau = kp.* (phi des phi) kd.* phi dot g fun(phi); end Exercise 2.2 Does the end-effector position converge to the desired position? What would happen if 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: τ = k p ( ϕ des ϕ) k d ϕ + g( ϕ) (6) Under this control action, at steady state (i.e. ϕ 0, ϕ 0) one has: Hence, by choosing k pi > 0 i, one has: kp ( ϕ des ϕ) = 0 (7) ϕ 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: τ = k p ( ϕ des ϕ) k d ϕ + ˆ g( ϕ) (9) where g( ϕ) R 6 is the available estimate of the effect of gravity on each joint. The equations of motion at steady state are now: The steady state is now: kp ( ϕ des ϕ) + g( ϕ) = g( ϕ) (10) ( ϕ des ϕ) = k 1 p ( g( ϕ) g( ϕ)) (11) where kp 1 is the 6 1 vector whose elements are the inverse of those of k p. This shows in the realistic case of imperfect cancellation of gravity the system exhibits a steady state error. This can be reduced by increasing the gains k pi, which in turn leads to a (potentially undesirable) more aggressive control action. 7

8 2.2 Inverse dynamics control Exercise 2.3 Implement a PD controller in the operational space that fully counteracts all the non-linearity of the system. This controller can be written as τ = C( ϕ, ϕ, x E,des ), where x E,des is the desired end effector position in the operational space. The PD controller here outputs the desired acceleration in operational space. Use the provided model function or Simulink block to test your controller. Solution 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 % Gains 7 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 orientation 11 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); % Define error orientation in Angle & Axis 19 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)); % 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; %Inverse dynamics 30 tau = M fun(phi) * phi dot dot + b fun(phi, phi dot) + g fun(phi); end Exercise 2.4 The simulator inputs a constant desired position and zero initial velocity to the controller. How would the end-effector trajectory look like (assuming perfect knowledge of the model)? Solution 2.4 The end effector position trajectory forms a line. The end effector orientation trajectory has a constant axis of rotation (i.e. the quaternion trajectory is a geodesic). 8

9 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 the window. This controller applies a constant force on the wall in x-axis and follows a trajectory defined on y,z-plane. It can be written as ~τ = C(~ ϕ, ϕ ~, ~xe,des, Fx ). PD controller 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 controller % Gains kp = 20 * [0,1,1,0.01,0.01,0.01]'; kd = 40.0 * [1,1,1,1,1,1]'; % Endeffector force F e = [Fx, 0, 0, 0, 0, 0]'; % Find jacobians, positions and orientation 9

10 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); % Project M b g to operational space 22 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); % Define error orientation in Angle & Axis 27 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)); % 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) F e + mu + p; % Map Force back to the joint space 38 tau = J e' * F e des; end 10

11 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 ϕ = (12) ϕ r 1 % define generalized coordinates 2 syms x q1 q2 real 3 phi = [x,q1,q2]'; 4 % define generalized velocities 5 syms Dx Dq1 Dq2 real 6 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 r OP = B r OP ( ϕ) (13) 1 % define certain parameters 2 syms l real 3 % position vector 4 r = [l*sin(q1),l*cos(q1),0]; 3.3 Velocity (in Moved Systems) The velocity is given through differentiation, whereby special attention is required when differentiating in a moving (with respect to inertial frame I) coordinates system B: d I r I r I r = dt d B r B r B r = dt (14) + B ω IB B r (15) In this document, O refers to the origin of a frame, I indicates the inertial frame and B a body fixed frame (which can be moved). In the case of rigid body kinematics, the body angular velocity is often indicated by Ω Ω = I ω IB (16) 1 % derivation of position vectors expressed in inertia frame 2 dr = dmatdt(r,q,dq); using the function 1 function [ da ] = dmatdt( A, q, dq ) 2 da = sym(zeros(size(a))); 11

12 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 end 7 end 8 end 3.4 Rotation B y I y B x I z B z O j I x Figure 3: Rotated coordinate system B around z axis The rotation matrix R IB rotates a vector B r expressed in frame B to frame I: I r = R IBB r (17) B r = R BI I r R BI = R T IB (18) C r = R CI I r R CI = R CB R BI (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 R IB, the corresponding rotation speed I Ω of a rigid body with body fixed frame B is: Ω I = I ω IB = ṘIBR T IB (20) 0 Ω z Ω y unskew Ω x Ω = Ω z 0 Ω x, Ω = Ω y (21) Ω y Ω x 0 skew Ω z 12

13 1 % rotation matrix around z with vector phi 2 RIB = eulertorotmat R IB(0,0,phi); 3 % differentiate rotation matrix 4 drib = dmatdt(drib,q,dq); 5 % generate rotation speed and unskew it 6 I Omega = unskew(simplify(daib*aib')); with the skew and unskew function 1 % skew function 2 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 function 8 function a=unskew(a) 9 a = [A(3,2); A(1,3); A(2,1)]; 3.6 Jacobian The Jacobian J is given through: r 1 r 1 ϕ 1 ϕ 2... r r ( ϕ) J ( ϕ) = ϕ = 2 r 2 ϕ 1 ϕ r m ϕ 2... r m ϕ 1 r 1 ϕ n r 2 ϕ n r m ϕ n, ϕ R n 1, r 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 τ : τ = J T F (24) We differ between translational Jacobians J P specific point P and rotational Jacobians J R points of one single rigid body. = r P ( ϕ) ϕ, which correspond to a = Ω( ϕ) ϕ that are identical for all 1 % get jacobian from position vector 2 J = jacobian(r,q); 3 % get jacobian from rotation speed 4 Jr = jacobian(omega,dq); 13