deleted autonomous.cpp and .h
KinematicData struct added

structure for forward kinematics function.
fixed bug

was missing a semicolon
added functionality to go to ready
changes to test application
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
gripper orientation difference
changed to use new control mode
added new control mode to enum ARM_FRAME_MAPPED=3
misc changes while testing optimize