boden_fix = {[-1.25 0 0;
1.25 0 0]};
boden_u = {[0.9 0 0;
1.2 0 0]};
karosserie_1 = {[-p.a/2 -p.hs 0;
p.a/2 -p.hs 0;
p.a/2 p.hs 0;
-p.a/2 p.hs 0;
-p.a/2 -p.hs 0]};
hebel_2 = {[ 0 0 0;
p.be 0 0]};
hebel_3 = {[ 0 0 0;
-p.be 0 0]};
hebel_5 = {[ 0 0 0;
-p.b5 0 0]};
hebel_7 = {[ 0 0 0;
p.b5 0 0]};
koerper_4 = {[ 0 0 0;
-p.b4/2 p.hs4 0;
p.bc p.hs4 0;
p.bc -p.hc4 0;
p.bc p.h4 0;
p.bc p.hs4 0;
-p.b4/2 p.hs4 0;
-p.b4 p.h4 0]};
koerper_6 = {[ 0 0 0;
p.b4/2 p.hs4 0;
-p.bc p.hs4 0;
-p.bc -p.hc4 0;
-p.bc p.h4 0;
-p.bc p.hs4 0;
p.b4/2 p.hs4 0;
p.b4 p.h4 0]};
fh_boden_fix = @(t,x)deal([0;0; 0],[1,0,0;0,1,0;0,0,1]);
fh_boden_u = @(t,x)deal([0;u_von_t(t);0],[1,0,0;0,1,0;0,0,1]);
fh_karosserie_1 = matlabFunction(I_r_OS1,A_IK1, 'vars', {t, q});
fh_hebel_2 = matlabFunction(I_r_OA, A_IK2, 'vars', {t, q});
fh_hebel_3 = matlabFunction(I_r_OB, A_IK3, 'vars', {t, q});
fh_hebel_5 = matlabFunction(I_r_OF, A_IK5, 'vars', {t, q});
fh_hebel_7 = matlabFunction(I_r_OJ, A_IK7, 'vars', {t, q});
fh_koerper_4 = matlabFunction(I_r_OE, A_IK4, 'vars', {t, q});
fh_koerper_6 = matlabFunction(I_r_OI, A_IK6, 'vars', {t, q});
t_start = 0;
t_end = 4;
x0 = [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0];
options = odeset('RelTol',1.e-4,...
'AbsTol',1.e-7,...
'OutputFcn',@odeplot);
h=figure;
tic
[t,x_] = ode23(@ode_fcn,[t_start t_end],x0,options);
toc
close(h);
for i=1:length(t)
x__p(i,:)=ode_fcn(t(i),x_(i,:)');
end
q = x_(:, 1:p.n);
q_p = x_(:, (p.n+1):p.n*2);
q_pp = x__p(:,(p.n+1):p.n*2);
for i=1:length(t)
a_p(i,:)=p.fhandle_ap(q(i,:)',q_p(i,:)',q_pp(i,:)')';
end
figure;
animation4(t,x(:,1:13),.05,{fh_boden_fix boden_fix 4;...
fh_boden_u boden_u 4;...
fh_karosserie_1 karosserie_1 3;...
fh_hebel_2 hebel_2 2;...
fh_hebel_3 hebel_3 2;...
fh_koerper_4 koerper_4 2;...
fh_hebel_5 hebel_5 2;...
fh_koerper_6 koerper_6 2;...
fh_hebel_7 hebel_7 2},[0,90]);
