MATLAB: Symbolic calculations for physical system doesn’t give apropriated answer.

Question

Hello,

I'm trying to solve the equations of motion of a triple inverted pendulum with matlab as in figure below

my code is as follows :

clear variables
digits(5);
syms phi1(t)
syms phi2(t)
syms phi3(t)
syms s(t)
syms u(t)
d1=0.215;
d2=0.002;
d3=0.002;
J1=0.013;
J2=0.024;
J3=0.018;
a1=0.215;
a2=0.269;
a3=0.226;
g=9.81;
l1=0.323;
l2=0.419;
m1=0.876;
m2=0.938;
m3=0.553;
mc=1; %actually we I didn't find the mass of the cart.
R=0.5*d1*diff(phi1,t)^2+0.5*d2*(diff(phi2,t)-diff(phi1,t))^2+0.5*d3*(diff(phi3,t)-diff(phi2,t))^2;
q=[phi1;phi2;phi3;s];
qdot=diff(q,t);
q=formula(q);
qdot=formula(qdot);
pc0=[s;0]; %position of the cart
pc1=[s-a1*sin(phi1);a1*cos(phi1)];
pc2=[s-l1*sin(phi1)-a2*sin(phi2);l1*cos(phi1)+a2*cos(phi2)];
pc3=[s-l1*sin(phi1)-l2*sin(phi2)-a3*sin(phi3);l1*cos(phi1)+l2*cos(phi2)+a3*cos(phi3)];
yc1=[0,1]*pc1;
yc2=[0,1]*pc2;
yc3=[0,1]*pc3;
vc1=diff(pc1,t);
vc2=diff(pc2,t);
vc3=diff(pc3,t);
vc1Norm2=transpose(vc1)*vc1;
vc1Norm2=simplify(vc1Norm2);
vc2Norm2=transpose(vc2)*vc2;
vc2Norm2=simplify(vc2Norm2);
vc3Norm2=transpose(vc3)*vc3;
vc3Norm2=simplify(vc3Norm2);
V=g*(m1*yc1+m2*yc2+m3*yc3);
T=0.5*(mc*diff(s,t)^2+m1*vc1Norm2+m2*vc2Norm2+m3*vc3Norm2+J1*diff(phi1,t)^2+J2*diff(phi2,t)^t+J3*diff(phi3,t)^2);
T=simplify(T);
L=T-V;
L=simplify(L);
R=0.5*(d1*diff(phi1,t)^2+d2*(diff(phi2,t)-diff(phi1,t))^2+d3*(diff(phi3,t)-diff(phi2,t))^2);
eulerLagrange=@(f,t,x,xd) diff(diffDepVar(f,xd),t)-diffDepVar(f,x)+diffDepVar(R,xd);
dL1=eulerLagrange(L,t,phi1,diff(phi1,t));
eqn1=dL1==0;
eqn1=simplify(eqn1);
dL2=eulerLagrange(L,t,phi2,diff(phi2,t));
eqn2=dL2==0;
eqn2=simplify(eqn2);
dL3=eulerLagrange(L,t,phi3,diff(phi3,t));
eqn3=dL3==0;
eqn3=simplify(eqn3);
eqn4=u==diff(diff(s,t),t);
[V,Y]=odeToVectorField(eqn1,eqn2,eqn3,eqn4);
V=subs(V,u,0);
f=matlabFunction(V,'vars',{'t','Y'});
tspan=[0,1];
y0=[0;0;0;0;0;0;pi/2;0];
[t,y]=ode45(f,tspan,y0);

for some reason, when I plot the answer y is a matrix containing more NaN than anything else. I thought that this may be due to the fact that f contains very big values (ex : 10^22). What do you think?

Answer 1

Check out the Euler-Lagrange tool package on File Exchange.