I think the grasp controller may be done.

Replaced TF nonsense with matrix math. Still need to run this on
the robot to see if it works.