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

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

430 D Programs of Chapter 5: Direct Dynamics

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

fprintf(fid,dx4dt);

fprintf(fid,’;’);

% closes the file associated with file identifier fid

fclose(fid);

cd(pwd);

% cd changes current working directory

% pwd displays the current working directory

% fid = fopen(FIL,PERM) opens the file FILE in the

% mode specified by PERM. PERM can be:

% ’w’ writes (creates if necessary)

% ’w+’ truncates or creates for read and write

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

x0 = [-pi/4 0 -pi/3 0]; % initial conditions

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

x1 = xs(:,1);

x3 = xs(:,3);

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,0:1:5,x0);

% [ts,xs]

% end of program

D.5 One-Link Planar Robot Arm

%D5

% Direct Dynamics

% R robot arm

clear all; clc; close all

syms t

L=1;m=1;g=9.81;

D.5 One-Link Planar Robot Arm 431

c = cos(sym(’theta(t)’));

s = sin(sym(’theta(t)’));

xC = (L/2)

c; yC = (L/2)

rC = [xC yC 0];

omega = [0 0 diff(’theta(t)’,t)];

alpha = diff(omega,t);

G=[0-m

g 0];

IO=m

Lˆ2/3;

beta = 45;

gamma = 30;

qf = pi/3;

T01z = -beta

diff(’theta(t)’,t)-...

gamma

(sym(’theta(t)’)-qf)+0.5

T01 = [0 0 T01z];

eq = -IO

alpha + cross(rC,G) + T01;

eqz = eq(3);

slist={diff(’theta(t)’,t,2),diff(’theta(t)’,t),...

’theta(t)’};

nlist={’ddtheta’, ’x(2)’ , ’x(1)’};

eqI = subs(eqz,slist,nlist);

dx1 = sym(’x(2)’);

dx2 = solve(eqI,’ddtheta’);

dx1dt = char(dx1);

dx2dt = char(dx2);

g=inline(sprintf(’[%s; %s]’, dx1dt, dx2dt), ’t’, ’x’);

time = [0 10];

x0 = [pi/18; 0]; % define initial conditions

[ts,xs] = ode45(g, 0:1:10, x0);

plot(ts,xs(:,1)

180/pi,’LineWidth’,1.5),...

xlabel(’t (s)’), ylabel(’\theta (deg)’),...

grid, axis([0, 10, 0, 70])

432 D Programs of Chapter 5: Direct Dynamics

fprintf(’Results \n\n’)

fprintf(’ t(s) theta(rad) omega(rad/s) \n’)

[ts,xs]

% end of program

Results:

t(s) theta(rad) omega(rad/s)

ans =

0 0.1745 0

1.0000 0.5984 0.3006

2.0000 0.8175 0.1539

3.0000 0.9297 0.0788

4.0000 0.9871 0.0403

5.0000 1.0164 0.0206

6.0000 1.0315 0.0105

7.0000 1.0391 0.0054

8.0000 1.0431 0.0028

9.0000 1.0451 0.0014

10.0000 1.0461 0.0007

D.6 One-Link Planar Robot Arm Using the m-File Function

Rrobot.m

%D6

% Direct Dynamics

% R robot arm

% the program uses the function: Rrobot(t,x)

% the function is defined in the program Rrobot.m

clear; clc; close all

time = [0 10];

x0 = [pi/18 0];

[ts,xs] = ode45(@Rrobot, 0:1:10, x0);

plot(ts,xs(:,1)

180/pi,’LineWidth’,1.5),...

xlabel(’t (s)’), ylabel(’\theta (deg)’),...

grid, axis([0, 10, 0, 70])

D.7 Two-Link Planar Robot Arm Using the m-File Function RRrobot.m 433

fprintf(’Results \n\n’)

fprintf(’ t(s) theta(rad) omega(rad/s) \n’)

[ts,xs]

% end of program

Results:

t(s) theta(rad) omega(rad/s)

ans =

0 0.1745 0

1.0000 0.5984 0.3006

2.0000 0.8175 0.1539

3.0000 0.9297 0.0788

4.0000 0.9871 0.0403

5.0000 1.0164 0.0206

6.0000 1.0315 0.0105

7.0000 1.0391 0.0054

8.0000 1.0431 0.0028

9.0000 1.0451 0.0014

10.0000 1.0461 0.0007

D.7 Two-Link Planar Robot Arm Using the m-File Function

RRrobot.m

%D7

% Direct Dynamics

% Double pendulum

% the program uses the function: RRrobot(t,x)

% the function is defined in the program RRrobot.m

clear all; clc; close all

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

t = sym(’t’,’real’);

xB=L1

cos(sym(’q1(t)’));

yB=L1

sin(sym(’q1(t)’));

rB = [xB yB 0];

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

xD=xB+L2

cos(sym(’q2(t)’));

yD=yB+L2

sin(sym(’q2(t)’));

rD = [xD yD 0];

434 D Programs of Chapter 5: Direct Dynamics

rC2 = (rB + rD)/2;

vC2 = diff(rC2,t); aC2 = diff(vC2,t);

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

alpha1 = diff(omega1,t);

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

alpha2 = diff(omega2,t);

G1=[0-m1

g 0]; G2 = [0 -m2

g 0];

IC1=m1

L1ˆ2/12; IA = IC1 + m1

(L1/2)ˆ2;

IC2=m2

L2ˆ2/12;

F21 = -m2

aC2 + G2;

b01 = 450; g01 = 300;

b12 = 200; g12 = 300;

q1f = pi/6;

q2f = pi/3;

T01z=-b01

diff(’q1(t)’,t)-g01

(sym(’q1(t)’)-q1f)...

+0.5

cos(sym(’q1(t)’))+...

cos(sym(’q1(t)’));

T01 = [0 0 T01z];

T12z = -b12

diff(’q2(t)’,t)-g12

(sym(’q2(t)’)-q2f)...

+0.5

cos(sym(’q2(t)’));

T12 = [0 0 T12z];

EqA=-IA

alpha1+cross(rB, F21)+cross(rC1, G1)+T01-T12;

Eq2=-IC2

alpha2 + cross(rB - rC2, -F21) + T12;

slist={diff(’q1(t)’,t,2),diff(’q2(t)’,t,2),...

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

nlist= ...

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

eq1 = subs(EqA(3),slist,nlist);

eq2 = subs(Eq2(3),slist,nlist);

sol = solve(eq1,eq2,’ddq1, ddq2’);

dx2 = sol.ddq1;

dx4 = sol.ddq2;

D.7 Two-Link Planar Robot Arm Using the m-File Function RRrobot.m 435

dx2dt = char(dx2);

dx4dt = char(dx4);

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

fprintf(fid,’function dx = RRrobot(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(@RRrobot, 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(@RRrobot,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

436 D Programs of Chapter 5: Direct Dynamics

Results:

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

ans =

0 -0.1745 0 0.5236 0

1.0000 0.1594 0.2373 0.9304 0.1758

2.0000 0.3327 0.1220 1.0213 0.0391

3.0000 0.4217 0.0626 1.0415 0.0087

4.0000 0.4674 0.0321 1.0460 0.0019

5.0000 0.4908 0.0165 1.0469 0.0004

Appendix E

Programs of Chapter 6: Analytical Dynamics

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

%E1

% Analytical Dynamics

% RR robot arm

% Lagrange’s e.o.m

clear all; clc; close all

symstL1L2m1m2g

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;

% kinetic energy of the link 1

T1=IA

omega1

omega1.’/2;

437

438 E Programs of Chapter 6: Analytical Dynamics

% .’ array transpose

% A.’ is the array transpose of A

% kinetic energy of the link 2

T2=m2

vC2

vC2.’/2 + IC2

omega2

omega2.’/2;

T2 = simple(T2);

% total kinetic energy

T = expand(T1 + T2);

fprintf(’T = \n’); pretty(T); fprintf(’\n’)

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

% dT/d(dq)

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

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

fprintf(’dT/d(dq1) = \n’); pretty(simple(Tdq1));

fprintf(’\n’)

fprintf(’dT/d(dq2) = \n’); pretty(simple(Tdq2));

fprintf(’\n’)

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

Tt1 = diff(Tdq1, t);

Tt2 = diff(Tdq2, t);

fprintf(’d dT/d(dq1)/dt = \n’);

pretty(simple(Tt1));

fprintf(’\n’)

fprintf(’d dT/d(dq2)/dt = \n’);

pretty(simple(Tt2));

fprintf(’\n’)

% dT/dq

Tq1 = deriv(T, q1);

Tq2 = deriv(T, q2);

fprintf(’dT/dq1 = \n’); pretty(Tq1);

fprintf(’\n’)

fprintf(’dT/dq2 = \n’); pretty(Tq2);

fprintf(’\n’)

% left-hand side of Lagrange’s eom

LHS1 = Tt1 - Tq1;

LHS2 = Tt2 - Tq2;

% generalized active forces

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

G1=[0-m1

g 0]; G2 = [0 -m2

g 0];

syms T01z T12z

% 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];

% partial derivatives

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));

% generalized active force Q1

Q1 = rC1_1

G1.’+w1_1

T01.’+w1_1

(-T12.’)+ ...

rC2_1

G2.’+w2_1

T12.’;

% generalized active force Q2

Q2 = rC1_2

G1.’+w1_2

T01.’+w1_2

(-T12.’)+ ...

rC2_2

G2.’+w2_2

T12.’;

fprintf(’Q1 = \n’); pretty(simple(Q1));

fprintf(’\n’)

fprintf(’Q2 = \n’); pretty(simple(Q2));

fprintf(’\n’)

% first Lagrange’s equation of motion

Lagrange1 = LHS1-Q1;

% second Lagrange’s equation of motion

Lagrange2 = LHS2-Q2;

% control torques

b01 = 450; g01 = 300;

b12 = 200; g12 = 300;

q1f = pi/6;

q2f = pi/3;