Last commit before clean up to find out why 0/0 cant be reached.

The problem of not swinging might be due to too large torque used. But
to check that, the 0/0 position must be reached.
Clean Up. Torque and Time Step adjusted for swing up.
Finished Clean Up.