Enable rotation/ motion system overhaul

[John-Bonnin]

The robot currently can't receive an orientation command relative to the hole. Changing orientations is dangerous, causing fast unplanned motion of the wrist as it orbits around the TCP. To enable this feature while mitigating the risk of damaging collision, we need a reliable and transparent system for setting the desired position command and controlling the motion speed. A new motion command system is probably needed.

[John-Bonnin]

Task has been recentered about target position as [0,0,0] for X and Y. Expected TCP orientation has been changed to have Z point up, matching the target's frame orientation, and the robot now rotates the TCP to match this orientation. Important: Make sure you flip your TCP orientations so Z points up! This is the opposite of the normal "robot flange Z = forward" convention.