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

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

C.7 R-RTR-RTR Mechanism: Contour Method 419

Fin1 = -m1 aC1 = [0.0213683, 0.012337, 0] (N)

G1=-m1g=[0,-0.117684, 0] (N)

IC1 = 2.26e-05 (kg mˆ2)

IC1 alpha1=[0, 0, 0](N m)

Min1=-IC1 alpha1=[0, 0, 0](N m)

Link 2

m2 = 0.008 (kg)

m2 aC2 = [-0.0284911, -0.0164493, 0] (N)

Fin2 = -m2 aC2 = [0.0284911, 0.0164493, 0] (N)

G2=-m2g=[0,-0.078456, 0] (N)

IC2 = 1.93333e-06 (kg mˆ2)

IC2 alpha2=[0, 0, 2.81035e-05](N m)

Min2=-IC2 alpha2=[0, 0, -2.81035e-05] (N m)

Link 3

m3 = 0.032 (kg)

m3 aC3 = [ -0.0272512, 0.0289293, 0] (N)

Fin3 = -m3 aC3 = [0.0272512, -0.0289293, 0] (N)

G3=-m3g=[0,-0.313824, 0] (N)

IC3 = 0.000426933 (kg mˆ2)

IC3 alpha3=[0, 0, 0.00620602](N m)

Min3=-IC3 alpha3=[0, 0, -0.00620602](N m)

Link 4

m4 = 0.008 (kg)

m4 aC4 = [ 0.0204384, -0.021697, 0] (N)

Fin4 = -m4 aC4 = [-0.0204384, 0.021697, 0] (N)

G4=-m4g=[0,-0.078456, 0] (N)

IC4 = 1.93333e-06 (kg mˆ2)

IC4 alpha4=[0, 0, 2.35748e-05](N m)

Min4=-IC4 alpha4=[0, 0, -2.35748e-05](N m)

Link 5

m5 = 0.024 (kg)

m5 aC5 = [-0.00475373, -0.0540826, 0] (N)

Fin5 = -m5 aC5 = [0.00475373, 0.0540826, 0] (N)

G5=-m5g=[0,-0.235368, 0] (N)

IC5 = 0.0001802 (kg mˆ2)

IC5 alpha5=[0, 0, 0.00219734](N m)

Min5=-IC5 alpha5=[0, 0, -0.00219734](N m)

Joint reactions

Contour method

420 C Programs of Chapter 4: Dynamic Force Analysis

Joint A_rotation (5 and 0)

.147297

F05x+.239677e-1-.128347

F05y = 0

.147297

F05y+.128347

F05x-100.008 = 0

F05 = [ 336.192, 386.015, 0] (N)

Joint D_translation

F45y-1.

F45x-99.9822 = 0

-1.

(xP+.147)

F45y+(yP-.128)

F45x-.236e-4 = 0

-.147297

F45x+.128347

F45y = 0

-.147297

yP-.128347

xP=0

F45 = [-336.196, -385.834, 0] (N)

rP = [-0.147297, 0.128347, 0] (m)

Joint D_rotation

-.147297

F34x-.427435e-2+.128347

F34y = 0

-99.9712-.147297

F34y-.128347

F34x = 0

F34 = [-336.176, -385.777, 0] (N)

Joint C_rotation

-.147297

F03x-38.5957+.283473e-1

F03y = 0

-124.851-.129904

F03y-.250000e-1

F03x = 0

F03 = [-431.027, -878.152, 0] (N)

Joint B_translation

F23y-1.

(yQ-.100000)

F23x-66.3764 = 0

-1.

(xQ-.130)

F23y+(yQ-.750e-1)

F23x-.281e-4 = 0

-.129904

F23x+.250000e-1

F23y = 0

.129904

yQ-.129904e-1+.250000e-1

xQ=0

F23 = [94.8233, 492.717, 0] (N)

rQ = [0.129904, 0.075, 0] (N)

Joint B_rotation

-.129904

F12x-.525127e-2+.250000e-1

F12y = 0

.129904

F12y+.250000e-1

F12x-66.3837 = 0

F12 = [ 94.7949, 492.779, 0] (N)

Mm = [0, 0, 56.9119] (N m)

Joint A_rotation (1 and 0)

C.7 R-RTR-RTR Mechanism: Contour Method 421

-.129904

F01y+.750000e-1

F01x+56.9195 = 0

-.129904

F01x-.106608e-1+.250000e-1

F01y = 0

F01 = [94.7736, 492.884, 0] (N)

Appendix D

Programs of Chapter 5: Direct Dynamics

D.1 Compound Pendulum

%D1

% Direct Dynamics

% Compound pendulum

clear all; clc; close all

symsLmgt

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

alpha = diff(omega,t);

% diff(X,’t’) or diff(X,sym(’t’))

% differentiates a symbolic expression

% X with respect to t

% diff(X,’t’,n) and diff(X,n,’t’)

% differentiates X n times

% n is a positive integer

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

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

xC = (L/2)

yC = (L/2)

rC = [xC yC 0];

G=[0-m

g 0];

IC=m

Lˆ2/12;

IO=IC+m

(L/2)ˆ2;

MO = cross(rC,G);

eq = -IO

alpha+MO;

eqz = eq(3);

eqI = subs(eqz,{L,m,g},{3,12/32.2,32.2});

eqI1 = subs(eqI,diff(’theta(t)’,t,2),’ddtheta’);

eqI2 = subs(eqI1,diff(’theta(t)’,t),sym(’x(2)’));

423

424 D Programs of Chapter 5: Direct Dynamics

eqI3 = subs(eqI2,’theta(t)’,sym(’x(1)’));

% solve a second order ODE using MATLAB

% ODE45 function

% write the second order equation as a

% system of two first order equations,

% by introducing x(2) = x(1)’

% x(1)’ = x(2) ==> x’ = g(t,x)

% x(2)’ = f

% define the vector x = [x(1); x(2)]

% (column-vector).

% first differential equation

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

% second differential equation

dx2 = solve(eqI3,’ddtheta’);

% define right-hand side vector

dx1dt = char(dx1);

dx2dt = char(dx2);

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

t0 = 0; % define initial time

tf = 10; % define final time

time = [0 tf];

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

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

% find t, xs, but don’t show

% ode45 solves non-stiff differential equations,

% medium order method.

% [ts,ys] = ode45(f,tspan,y0) integrates

% the system of differential equations

% y’ = f(t,y) with initial conditions y0.

x1 = xs(:,1); % extract x1 & x2 components from xs

x2 = xs(:,2);

subplot(3,1,1),plot(t,x1,’r’),...

xlabel(’t’),ylabel(’\theta’),grid,...

subplot(3,1,2),plot(t,x2),...

xlabel(’t’),ylabel(’\omega’),grid,...

D.2 Compound Pendulum Using the Function R(t,x) 425

subplot(3,1,3),plot(x1,x2),...

xlabel(’\theta’),ylabel(’\omega’),grid

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

format short

[ts,xs]

% end of program

Results:

ans =

0 0.7854 0

0.5000 -0.8609 -6.8742

1.0000 -3.7001 -2.3851

1.5000 -3.3568 3.9823

2.0000 -0.1916 5.3803

2.5000 0.7178 -1.2627

3.0000 -1.6474 -7.4097

3.5000 -3.8826 -1.0102

4.0000 -2.8311 5.7023

4.5000 0.2980 3.6596

5.0000 0.5131 -2.6538

5.5000 -2.4083 -6.6686

6.0000 -3.9189 0.2451

6.5000 -2.1153 7.0898

7.0000 0.6019 2.0283

7.5000 0.0638 -4.5067

8.0000 -3.1170 -4.8067

8.5000 -3.7773 1.7866

9.0000 -1.1661 7.2102

9.5000 0.7582 0.4365

10.0000 -0.6490 -6.4667

D.2 Compound Pendulum Using the Function R(t,x)

%D2

% Direct Dynamics

% Compound pendulum

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

% the function is defined in the program R.m

clear all; clc; close all

426 D Programs of Chapter 5: Direct Dynamics

tfinal=10;

time=[0 tfinal];

% x(1)(0)=pi/4 x(2)(0)=0

x0=[pi/4 0];

% solve a second order ODE

% using MATLAB ODE45 function

[t,x]=ode45(@R,time,x0);

subplot(3,1,1),...

plot(t,x(:,1),’r’),...

xlabel(’t’),ylabel(’\theta’),grid,...

subplot(3,1,2),...

plot(t,x(:,2)),...

xlabel(’t’),ylabel(’\omega’),grid,...

subplot(3,1,3),...

plot(x(:,1),x(:,2)),...

xlabel(’\theta’), ylabel(’\omega’),grid

% end of program

D.3 Double Pendulum

%D3

% Direct Dynamics

% Double pendulum

clear all; clc; close all

L1 = 1; L2 = 0.5; m1 = 1; m2 = 1;g=10;

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

xB=L1

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

yB=L1

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

rB = [xB yB 0];

rC1 = rB/2;

% differentiates rC1 with respect to t

vC1 = diff(rC1,t);

aC1 = diff(vC1,t);

xD=xB+L2

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

yD=yB+L2

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

D.3 Double Pendulum 427

rD = [xD yD 0];

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;

% LINK 2

% Sum F for link 2:

% -m2 aC2 + G2 + (-F21)=0=>F21=-m2aC2+G2

F21 = -m2

aC2 + G2;

% LINK 1

% Sum M for 1 wrt A:"

% -IA alpha1 + AB x F21 + AC1 x G1 = 0

EqA = -IA

alpha1 + cross(rB, F21) + cross(rC1, G1);

% LINK 2

% Sum M for 2 wrt C2:

% -IC2 alpha2 + C2B x (-F21) = 0

Eq2 = -IC2

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

% list for the symbolical variables

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

% diff(’q1(t)’,t,2) will be replaced by ’ddq1’

% diff(’q2(t)’,t,2) will be replaced by ’ddq2’

% diff(’q1(t)’,t) will be replaced by ’x(2)’

% diff(’q2(t)’,t) will be replaced by ’x(4)’

% ’q1(t)’ will be replaced by ’x(1)’

% ’q2(t)’ will be replaced by ’x(3)’

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

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

428 D Programs of Chapter 5: Direct Dynamics

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

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

dx2 = sol.ddq1;

dx3 = sym(’x(4)’);

dx4 = sol.ddq2;

dx1dt = char(dx1);

dx2dt = char(dx2);

dx3dt = char(dx3);

dx4dt = char(dx4);

g=inline(sprintf(’[%s;%s;%s;%s]’,...

dx1dt,dx2dt,dx3dt,dx4dt),’t’,’x’);

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

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

[t,xs] = ode45(g, 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(g,0:1:5,x0);

%[ts,xs]

% end of program

D.4 Double Pendulum Using the File RR.m

%D4

% Direct Dynamics

% Double pendulum

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

% the function is defined in the program RR.m

D.4 Double Pendulum Using the File RR.m 429

clear all; clc; close all

L1 = 1; L2 = 0.5; m1 = 1; m2 = 1;g=10;

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

xB=L1

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

yB=L1

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

rB = [xB yB 0];

rC1 = rB/2;

xD = xB+L2

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

yD = yB+L2

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

rD = [xD yD 0];

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;

EqA = -IA

alpha1 + cross(rB, F21) + cross(rC1, G1);

Eq2 = -IC2

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

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;

dx2dt = char(dx2);

dx4dt = char(dx4);

% opens the file ’RR.m’ in the mode specified by ’w+’

% (create for read and write)

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

fprintf(fid,’function dx = RR(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’);