Changed optimize to work on current joint angles

created a new Opt2() function that calculates weights based on
current(next) joint angles, instead of previous joint angles.
added features

specify coordinate frame of motion
go to ready position
get pose from user.
display current pose before motion
uses M_PI from math.h

now uses M_PI from math.h so it is more accurate
some additional changes to help debugging
changed joint acceleration