Mixed control and mechanics problems like controlling an inverted pendulum, a robotic arm or a flying drones gives rise to mechanical problems that can be solved by standard methods but the calculations can get quite involved and translating the formulas into C code without errors needs very careful work.The free CAS system Maxima can help us in the calculations and also generate almost runnable code expressions. The following example shows how the equations of motion for an inverted pendulum can be derived with the aid of Maxima.
Deriving the equations of motion in Maxima
The first command tells Maxima that p1,p2, p3 and p4 are functions of t, this important for taking derivatives later. The next line is the Lagrangian.depends([p1,p2,q1,q2],t);
L:M*p1^2/2+1/2*m*((p1+l*p2*cos(q2))^2+(l*p2*sin(q2))^2+I*p2^2)-m*g*l*cos(q2);
The equations of motion are given by
\( \frac{d}{dt} \left [ \frac{\partial L}{\partial p_i} \right ] - \frac{\partial L}{\partial q_i} = F_i \)
eq1:ratsimp(diff(diff(L,p1),t)-diff(L,q1)=F);
eq2:ratsimp(diff(diff(L,p2),t)-diff(L,q2)=0);
Rewriting the equations of motions as a dynamical system
ds1:ev(solve([eq1,eq2],[diff(p1,t),diff(p2,t)]),[diff(q1,t)=p1,diff(q2,t)=p2]);
ds2:trigsimp(ds1);
The resulting dynamical system where the definitions of p1 and p2 as derivatives of q1 and q2 has been inserted:
\( \begin{align*}
\frac{d}{d\,t}\,q1 &=p1 &\\
\frac{d}{d\,t}\,p1 &=\frac{\left( F+l\,m\,{p2}^{2}\,\mathrm{sin}\left( q2\right) \right) \,I+{l}^{2}\,F+\left( {l}^{3}\,m\,{p2}^{2}−g\,{l}^{2}\,m\,\mathrm{cos}\left( q2\right) \right) \,\mathrm{sin}\left( q2\right) }{\left( I+{l}^{2}\right) \,M+m\,I+{l}^{2}\,m\,{\mathrm{sin}\left( q2\right) }^{2}} \\
\frac{d}{d\,t}\,q2 &=p2 \\
\frac{d}{d\,t}\,p2 &=\frac{g\,l\,\mathrm{sin}\left( q2\right) \,M−l\,\mathrm{cos}\left( q2\right) \,F+\left( g\,l\,m−{l}^{2}\,m\,{p2}^{2}\,\mathrm{cos}\left( q2\right) \right) \,\mathrm{sin}\left( q2\right) }{\left( I+{l}^{2}\right) \,M+m\,I+{l}^{2}\,m\,{\mathrm{sin}\left( q2\right) }^{2}}
\end{align*} \)
Converting our system into C code
The Maxima 'fortran' command gives us an output that quite easily can be changed into a usable C formula.fortran(ds2);
ds2(1) = ['diff(p1,t,1) = ((F+l*m*p2**2*sin(q2))*I+l**2*F+(l**3*m*
1 p2**2-g*l**2*m*cos(q2))*sin(q2))/((I+l**2)*M+m*I+l**2*m*sin(q2)
2 **2),'diff(p2,t,1) = (g*l*sin(q2)*M-l*cos(q2)*F+(g*l*m-l**2*m*p
3 2**2*cos(q2))*sin(q2))/((I+l**2)*M+m*I+l**2*m*sin(q2)**2)]
There are some Fortran constructs to remove, like line numbers and ** as the power operator, but all the elements of the formulas are in place.
depends([p1,p2,q1,q2],t);L:M*p1^2/2+1/2*m*((p1+l*p2*cos(q2))^2+(l*p2*sin(q2))^2+I*p2^2)-m*g*l*cos(q2);
eq1:ratsimp(diff(diff(L,p1),t)-diff(L,q1)=F);
eq2:ratsimp(diff(diff(L,p2),t)-diff(L,q2)=0);
ds1:ev(solve([eq1,eq2],[diff(p1,t),diff(p2,t)]),[diff(q1,t)=p1,diff(q2,t)=p2]);
ds2:trigsimp(ds1);
fortran(ds2);
A good discussion of using Maxima together with Octave/Matlab can be found here:
http://math-blog.com/using-maxima-output-in-octave/
