Dan B. Marghitu, Mechanisms and Robots Analysis with MATLAB®

Подождите немного. Документ загружается.

440 E Programs of Chapter 6: Analytical Dynamics

T01zc = -b01

diff(q1,t)-g01

(q1-q1f)+ ...

0.5

c1+g

c1;

T12zc = -b12

diff(q2,t)-g12

(q2-q2f)+ ...

0.5

c2;

tor = {T01z, T12z};

torf = {T01zc,T12zc};

Lagrang1 = subs(Lagrange1, tor, torf);

Lagrang2 = subs(Lagrange2, tor, torf);

data = {L1, L2, m1, m2, g};

datn={1,1,1,1,9.81};

% L1 = 1; L2 = 1; m1 = 1; m2 = 1; g = 9.81;

Lagran1 = subs(Lagrang1, data, datn);

Lagran2 = subs(Lagrang2, data, datn);

ql = {diff(q1,t,2), diff(q2,t,2),...

diff(q1,t), diff(q2,t), q1, q2};

qf = ...

{’ddq1’, ’ddq2’, ’x(2)’, ’x(4)’, ’x(1)’, ’x(3)’};

%ql qf

%----------------------------

% diff(’q1(t)’,t,2) -> ’ddq1’

% diff(’q2(t)’,t,2) -> ’ddq2’

% diff(’q1(t)’,t) -> ’x(2)’

% diff(’q2(t)’,t) -> ’x(4)’

% ’q1(t)’ -> ’x(1)’

% ’q2(t)’ -> ’x(3)’

Lagra1 = subs(Lagran1, ql, qf);

Lagra2 = subs(Lagran2, ql, qf);

% solve e.o.m. for ddq1, ddq2

sol = solve(Lagra1,Lagra2,’ddq1, ddq2’);

Lagr1 = sol.ddq1;

Lagr2 = sol.ddq2;

% system of ODE

dx2dt = char(Lagr1);

dx4dt = char(Lagr2);

E.1 Lagrange’s Equations for Two-Link Robot Arm 441

fid = fopen(’RR_Lagr.m’,’w+’);

fprintf(fid,’function dx = RR_Lagr(t,x)\n’);

fprintf(fid,’dx = zeros(4,1);\n’);

fprintf(fid,’dx(1) = x(2);\n’);

fprintf(fid,’dx(2) = ’);

fprintf(fid,dx2dt);

fprintf(fid,’;\n’);

fprintf(fid,’dx(3) = x(4);\n’);

fprintf(fid,’dx(4) = ’);

fprintf(fid,dx4dt);

fprintf(fid,’;’);

fclose(fid); cd(pwd);

t0 = 0; tf = 15; time = [0 tf];

x0 = [pi/18 0 pi/6 0];

[t,xs] = ode45(@RR_Lagr, time, x0);

x1 = xs(:,1);

x2 = xs(:,2);

x3 = xs(:,3);

x4 = xs(:,4);

subplot(2,1,1),plot(t,x1

180/pi,’r’),...

xlabel(’t (s)’),ylabel(’q1 (deg)’),grid,...

subplot(2,1,2),plot(t,x3

180/pi,’b’),...

xlabel(’t (s)’),ylabel(’q2 (deg)’),grid

[ts,xs] = ode45(@RR_Lagr,0:1:5,x0);

fprintf(’Results \n\n’)

fprintf ...

(’t(s) q1(rad) dq1(rad/s) q2(rad) dq2(rad/s) \n’)

[ts,xs]

% end of program

Numerical results:

t(s) q1(rad) dq1(rad/s) q2(rad) dq2(rad/s)

ans =

0 0.1745 0 0.5236 0

1.0000 0.3387 0.1175 0.9305 0.1757

2.0000 0.4246 0.0605 1.0213 0.0390

442 E Programs of Chapter 6: Analytical Dynamics

3.0000 0.4688 0.0311 1.0415 0.0087

4.0000 0.4915 0.0160 1.0459 0.0019

5.0000 0.5032 0.0082 1.0469 0.0004

E.2 Two-Link Robot Arm: Inverse Dynamics

%E2

% Analytical Dynamics

% RR robot arm

% Lagrange’s e.o.m

% Inverse dynamics

clear all; clc; close all

symstL1L2 m1m2m3g

q1 = sym(’q1(t)’); q2 = sym(’q2(t)’);

c1 = cos(q1); s1 = sin(q1);

c2 = cos(q2); s2 = sin(q2);

xB=L1

c1; yB = L1

s1;

rB = [xB yB 0];

rC1 = rB/2; vC1 = diff(rC1,t);

xD=xB+L2

c2;yD=yB+L2

s2;

rD = [xD yD 0];

rC2 = (rB + rD)/2; vC2 = diff(rC2,t);

omega1 = [0 0 diff(q1,t)];

omega2 = [0 0 diff(q2,t)];

IA=m1

L1ˆ2/3; IC2 = m2

L2ˆ2/12;

T1=IA

omega1

omega1.’/2;

T2=m2

vC2

vC2.’/2 + IC2

omega2

omega2.’/2;

T = expand(T1 + T2);

%deriv(f,g(t)) differentiates f with respect to g(t)

Tdq1 = deriv(T, diff(q1,t));

Tdq2 = deriv(T, diff(q2,t));

Tt1 = diff(Tdq1, t); Tt2 = diff(Tdq2, t);

Tq1 = deriv(T, q1); Tq2 = deriv(T, q2);

LHS1 = Tt1 - Tq1; LHS2 = Tt2 - Tq2;

rC1_1 = deriv(rC1, q1); rC2_1 = deriv(rC2, q1);

rC1_2 = deriv(rC1, q2); rC2_2 = deriv(rC2, q2);

w1_1 = deriv(omega1, diff(q1,t));

w2_1 = deriv(omega2, diff(q1,t));

w1_2 = deriv(omega1, diff(q2,t));

w2_2 = deriv(omega2, diff(q2,t));

G1=[0-m1

g 0]; G2 = [0 -m2

g 0];

syms T01z T12z

E.2 Two-Link Robot Arm: Inverse Dynamics 443

% contact torque of 0 that acts on link 1

T01 = [0 0 T01z];

% contact torque of link 1 that acts on link 2

T12 = [0 0 T12z];

Q1=rC1_1

G1.’+w1_1

T01.’+w1_1

(-T12.’)+ ...

rC2_1

G2.’+w2_1

T12.’;

Q2=rC1_2

G1.’+w1_2

T01.’+w1_2

(-T12.’)+ ...

rC2_2

G2.’+w2_2

T12.’;

Lagrange1 = LHS1-Q1; Lagrange2 = LHS2-Q2;

data = {L1, L2, m1, m2, g};

datn={1,1,1,1,9.81};

Lagr1 = subs(Lagrange1, data, datn);

Lagr2 = subs(Lagrange2, data, datn);

% solve for T01z T12z

sol = solve(Lagr1,Lagr2,’T01z, T12z’);

T01zc = sol.T01z;

T12zc = sol.T12z;

% INVERSE DYNAMICS

q1f = pi/6 ; q2f = pi/3;

q1s = pi/18; q2s = pi/6;

Tp=15.;

q1n=q1s+(q1f-q1s)/Tp

(t-Tp/(2

pi)

sin(2

pi/Tp

t));

q2n=q2s+(q2f-q2s)/Tp

(t-Tp/(2

pi)

sin(2

pi/Tp

t));

dq1n = diff(q1n,t);

dq2n = diff(q2n,t);

ddq1n = diff(dq1n,t);

ddq2n = diff(dq2n,t);

ql={diff(q1,t,2),diff(q2,t,2), ...

diff(q1,t),diff(q2,t),q1,q2};

qn={ddq1n, ddq2n, dq1n, dq2n, q1n, q2n};

%ql qn

%----------------------------

% diff(’q1(t)’,t,2) -> ddq1n

% diff(’q2(t)’,t,2) -> ddq2n

% diff(’q1(t)’,t) -> dq1n

444 E Programs of Chapter 6: Analytical Dynamics

% diff(’q2(t)’,t) -> dq2n

% ’q1(t)’ -> q1n

% ’q2(t)’ -> q1n

T01zt = subs(T01zc, ql, qn);

T12zt = subs(T12zc, ql, qn);

% ezplot(f,[min,max]) plots f(t)

% over the domain: min<t<max

subplot(2,1,1), ezplot(T01zt,[0,Tp]),...

xlabel(’t (s)’), ylabel(’T01z (N m)’),...

title(’’), grid,...

subplot(2,1,2),ezplot(T12zt,[0,Tp]),...

xlabel(’t (s)’), ylabel(’T12z (N m)’),...

title(’’), grid

% % another way of plotting T01z and T12z

% time = 0:1:Tp;

% T01t = subs(T01zt,’t’,time);

% T12t = subs(T12zt,’t’,time);

% subplot(2,1,1),plot(time,T01t),...

% xlabel(’t (s)’),ylabel(’T01z (N m)’),grid,...

% subplot(2,1,2),plot(time,T12t),...

% xlabel(’t (s)’),ylabel(’T12z (N m)’),grid

% q_1t = subs(q1n,’t’,time);

% q_2t = subs(q2n,’t’,time);

% subplot(2,1,1),plot(time,q_1t

180/pi),...

% xlabel(’t (s)’),ylabel(’q1 (deg)’),grid,...

% subplot(2,1,2),plot(time,q_2t

180/pi),...

% xlabel(’t (s)’),ylabel(’q2 (deg)’),grid

% end of program

E.3 RRT Robot Arm

%E3

% Analytical Dynamics

% RRT robot arm

E.3 RRT Robot Arm 445

% Lagrange’s e.o.m

clear all; clc; close all

syms t L1 L2 m1 m2 m3 g real

q1 = sym(’q1(t)’);

q2 = sym(’q2(t)’);

q3 = sym(’q3(t)’);

c1 = cos(q1);

s1 = sin(q1);

c2 = cos(q2);

s2 = sin(q2);

% transformation matrix from RF1 to RF0

R10 = [[1 0 0]; [0 c1 s1]; [0 -s1 c1]];

% transformation matrix from RF2 to RF1

R21 = [[c2 0 -s2]; [0 1 0]; [s2 0 c2]];

% angular velocity of link 1 in RF0 expressed

% in terms of RF1 {i1,j1,k1}

w10 = [diff(q1,t)00];

% angular velocity of link 2 in RF0 expressed

% in terms of RF1 {i1,j1,k1}

w201 = [diff(q1,t) diff(q2,t) 0];

% angular velocity of link 2 in RF0 expressed

% in terms of RF2 {i2,j2,k2}

w20 = w201

transpose(R21);

% angular acceleration of link 1 in RF0 expressed

% in terms of RF1{i1,j1,k1}

alpha10 = diff(w10,t);

% angular acceleration of link 2 in RF0 expressed

% in terms of RF2{i2,j2,k2}

alpha20 = diff(w20,t);

% position vector of mass center C1 of link 1

% in RF0 expressed in terms of RF1 {i1,j1,k1}

rC1 = [0 0 L1];

% linear velocity of mass center C1 of link 1

446 E Programs of Chapter 6: Analytical Dynamics

% in RF0 expressed in terms of RF1 {i1,j1,k1}

vC1 = diff(rC1,t) + cross(w10, rC1);

% position vector of mass center C2 of link 2

% in RF0 expressed in terms of RF2 {i2,j2,k2}

rC2=[002

L1]

transpose(R21) + [0 0 L2];

% linear velocity of mass center C2 of link 2

% in RF0 expressed in terms of RF2 {i2,j2,k2}

vC2 = simple(diff(rC2,t) + cross(w20,rC2));

% position vector of mass center C3 of link 3 in RF0

% expressed in terms of RF2 {i2,j2,k2}

rC3 = rC2 + [0 0 q3];

% linear velocity of mass center C3 of link 3 in RF0

% expressed in terms of RF2 {i2,j2,k2}

vC3 = simple(diff(rC3,t) + cross(w20,rC3));

% linear velocity of C32 of link 2 in RF0

% expressed in terms of RF2 {i2,j2,k2}

% C32 of link 2 is superposed with C3 of link 3

vC32 = simple(vC2 + cross(w20,[0 0 q3]));

% another way of computing vC3 is:

% vC3p= vC32+diff([0 0 sym(’q3(t)’)],t);

% vC3-vC3p

% linear accelerations

aC1 = simple(diff(vC1,t)+cross(w10,vC1));

aC2 = simple(diff(vC2,t)+cross(w20,vC2));

aC3 = simple(diff(vC3,t)+cross(w20,vC3));

% gravitational force that acts on link 1 at C1

% RF0 expressed in terms of RF1 {i1,j1,k1}

G1 = [-m1

g00]

% gravitational force that acts on link 2 at C2

% in RF0 expressed in terms of RF2 {i2,j2,k2}

G2 = [-m2

g00]

transpose(R21)

% gravitational force that acts on link 3 at C3

% in RF0 expressed in terms of RF2 {i2,j2,k2}

G3 = [-m3

g00]

transpose(R21)

E.3 RRT Robot Arm 447

syms T01x T01y T01z T12x T12y T12z F23x F23y F23z

% contact torque of 0 that acts on link 1

% in RF0 expressed in terms of RF1 {i1,j1,k1}

T01 = [T01x T01y T01z];

% contact torque of link 1 that acts on link 2

% in RF0 expressed in terms of RF2 {i2,j2,k2}

T12 = [T12x T12y T12z];

% contact force of link 2 that acts on link 3 at C3

% in RF0 expressed in terms of RF2 {i2,j2,k2}

F23 = [F23x F23y F23z];

%deriv(f,g(t)) differentiates f with respect to g(t)

w1_1 = deriv(w10, diff(q1,t));

w1_2 = deriv(w10, diff(q2,t));

w1_3 = deriv(w10, diff(q3,t));

w2_1 = deriv(w20, diff(q1,t));

w2_2 = deriv(w20, diff(q2,t));

w2_3 = deriv(w20, diff(q3,t));

vC1_1 = deriv(vC1, diff(q1,t));

vC1_2 = deriv(vC1, diff(q2,t));

vC1_3 = deriv(vC1, diff(q3,t));

vC2_1 = deriv(vC2, diff(q1,t));

vC2_2 = deriv(vC2, diff(q2,t));

vC2_3 = deriv(vC2, diff(q3,t));

vC32_1 = deriv(vC32, diff(q1,t));

vC32_2 = deriv(vC32, diff(q2,t));

vC32_3 = deriv(vC32, diff(q3,t));

vC3_1 = deriv(vC3, diff(q1,t));

vC3_2 = deriv(vC3, diff(q2,t));

vC3_3 = deriv(vC3, diff(q3,t));

% generalized active forces

% generalized active force Q1

Q1 = w1_1

T01.’ + vC1_1

G1.’ + ...

w1_1

transpose(R21)

(-T12.’) + ...

448 E Programs of Chapter 6: Analytical Dynamics

w2_1

T12.’ + vC2_1

G2.’ + vC32_1

(-F23.’) + ...

vC3_1

F23.’ + vC3_1

G3.’

% generalized active force Q2

Q2 = w1_2

T01.’ + vC1_2

G1.’ + ...

w1_2

transpose(R21)

(-T12.’) + ...

w2_2

T12.’ + vC2_2

G2.’ + vC32_2

(-F23.’) + ...

vC3_2

F23.’ + vC3_2

G3.’

% generalized active force Q3

Q3 = w1_3

T01.’ + vC1_3

G1.’ + ...

w1_3

transpose(R21)

(-T12.’) + ...

w2_3

T12.’ + vC2_3

G2.’ + vC32_3

(-F23.’) + ...

vC3_3

F23.’ + vC3_3

G3.’

% kinetic energy

% inertia dyadic

% inertia matrix associated with central inertia

% dyadic for link 1 expressed in terms of RF1

I1 = [m1

L1)ˆ2/12 0 0; 0 m1

L1)ˆ2/12 0;000];

% inertia matrix associated with central inertia

% dyadic for link 2 expressed in terms of RF2

I2 = [m2

L2)ˆ2/12 0 0; 0 m2

L2)ˆ2/12 0;000];

% inertia matrix associated with central inertia

% dyadic for link 3 expressed in terms of RF2

syms I3x I3y I3z real

I3 = [I3x 0 0; 0 I3y 0; 0 0 I3z];

% kinetic energy of the link 1

T1 = (1/2)

vC1

vC1.’ + (1/2)

w10

w10.’;

% kinetic energy of the link 2

T2 = (1/2)

vC2

vC2.’ + (1/2)

w20

w20.’;

% kinetic energy of the link 3

T3 = (1/2)

vC3

vC3.’ + (1/2)

w20

w20.’;

% .’ array transpose

% A.’ is the array transpose of A

% total kinetic energy

E.3 RRT Robot Arm 449

T = expand(T1 + T2 + T3);

% dT/d(dq)

Tdq1 = deriv(T, diff(q1,t));

Tdq2 = deriv(T, diff(q2,t));

Tdq3 = deriv(T, diff(q3,t));

% d(dT/d(dq))/dt

Tt1 = diff(Tdq1, t);

Tt2 = diff(Tdq2, t);

Tt3 = diff(Tdq3, t);

% dT/dq

Tq1 = deriv(T, q1);

Tq2 = deriv(T, q2);

Tq3 = deriv(T, q3);

% left-hand side of Lagrange’s eom

LHS1 = Tt1 - Tq1;

LHS2 = Tt2 - Tq2;

LHS3 = Tt3 - Tq3;

% first Lagrange’s equation of motion

Lagrange1 = LHS1-Q1;

% second Lagrange’s equation of motion

Lagrange2 = LHS2-Q2;

% third Lagrange’s equation of motion

Lagrange3 = LHS3-Q3;

% control torques and control force

q1f=pi/3; q2f=pi/3; q3f=0.3;

b01=450; g01=300;

b12=200; g12=300;

b23=150; g23=50;

T01xc = -b01

diff(q1,t)-g01

(q1-q1f);

T12yc = -b12

diff(q2,t)-g12

(q2-q2f)+ ...

(m2

L2+m3

(L2+q3))

c2;

F23zc = -b23

diff(q3,t)-g23

(q3-q3f)+g

s2;

tor = {T01x, T12y, F23z};

torf = {T01xc,T12yc,F23zc};

Lagrang1 = subs(Lagrange1, tor, torf);

Lagrang2 = subs(Lagrange2, tor, torf);