Inverted pendulum python simulation