Added some functions, clean up and comments

Functions added: initGP, getNextTarget, getNextStartPoint,
iDyn<-unfinished.
Changing to all reverse dynamics from here on. Forward dynamics will
not work due to  lack of knowledge of angular acceleration.