Inverted pendulum car - pedulum will not swing

[bolly]

Dear Community,

I am trying to implement the inverted pendulum cart simulation. To do so I’ve used the equation system 5 published here http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.619.551&rep=rep1&type=pdf
I've solved the equation by left hand multiplication with the inverse which eventually yield following ODE System (consisting of the equation for d^2Θ/dt^2) angular- and (d^2 X/dt^2) linear acceleration with the pendulum angle Θ, car position x, force F, pendulum length l, pendulum mass mp, car mass mc, g-force g:

d^2 X .. 7 F + mp ( 7 (dΘ/dt)^2 l - 3 g cos(Θ)) sin (Θ)
-------= --------------------------------------------------------------------------------
d^2t .. 7 l (mc + mp) - 3 l mp (cos(Θ))^2

d^2 Θ .. 3 g ( mc + mp ) sin(Θ) - 3 cos(Θ) (F + (dΘ/dt)^2 l mp sin(Θ))
-------= --------------------------------------------------------------------------------
d^2t .. 7 (mc + mp) - 3 mp (cos(Θ))^2

To numerical solve this ODE I’ve used the taylor chain for x and Θ:

Θ (t +dt) = Θ(t) + dt * dΘ/dt + dt^2 * d^2Θ/dt^2
X (t +dt) = X(t) + dt * dX/dt + dt^2 * d^2X/dt^2

with
dΘ/dt = ( Θ (t +dt) - Θ (t ) / dt
dX/dt = ( X (t +dt) - X (t ) / dt

and implemented the following C-code:

       double dTHETA2(double THETA, double dTHETA, double F, double dx)
       {
           return (3*g*(m_p+m_c)*Math.Sin(THETA)-3*Math.Cos(THETA)*(F+dTHETA*dTHETA*l*m_p*Math.Sin(THETA)))/(7*l*(m_c+m_p)-3*l*m_p+Math.Cos(THETA)*Math.Cos(THETA) );
       }

       double dX2(double F, double dx, double dTHETA2, double THETA, double dTHETA)
       {
           return (7*F+m_p*(7*dTHETA*dTHETA*l-3*g*Math.Cos(THETA))*Math.Sin(THETA))/( 7*(m_c+m_p)-3*m_p*Math.Cos(THETA)*Math.Cos(THETA) );
       }

         public void PerformProcessStep(double F)
        {

            double THETA_tpdt, X_tpdt;

         
            d2THETA_t = dTHETA2(THETA_t, dTHETA_t, F, dX_t);
            d2X_t = dX2(F, dX_t, d2THETA_t, THETA_t, dTHETA_t);

            THETA_tpdt = THETA_t + d2THETA_t * dt * dt / 2;
            X_tpdt = X_t + d2X_t * dt * dt / 2;

            dTHETA_t = (THETA_tpdt - THETA_t) / dt;
            dX_t = (X_tpdt - X_t) / dt;

            X_t = X_tpdt;
            THETA_t = THETA_tpdt;
        }

The problem is that my pendulum car behaves not as expected - after starting the pendulum from e.g. 10° off the vertical axis it starts to swing and the car also starts to move but then if the pendulum reaches 180° its angular velocity asymptotically approaches zero which shouldn’t be.

I would be glad if someone could give a hint

 

[bolly]

Dear Community,

after searching the web I found this https://rosettacode.org/wiki/Animate_a_pendulum#C.23 and adapted their method to numerically integrate an ODE - just for the case someone else needs some working code - here the simple solver:

         public void PerformProcessStep(double F)
        {

            d2THETA_t = dTHETA2(THETA_t, dTHETA_t, F, dX_t);
            d2X_t = dX2(F, dX_t, d2THETA_t, THETA_t, dTHETA_t);

            dTHETA_t += d2THETA_t * dt;
            dX_t += d2X_t * dt;

            THETA_t += dTHETA_t*dt;
            X_t += dX_t * dt;
        }[\CODE]

 

[jedishrfu]

Did you figure out why your original code didn't work?