New angle computation code.
Convert local three-space vector to radian value

Sets the ROS motor position to the radian value of the joint's
local position matrix.
Merge pull request #1 from hansonrobotics/new_angle_calc

New angle calc