Tool orientation quaternion math should be checked

While you can convert a rot matrix into a quat with Eigen's q = quaterniond(rotmatrix), I don't think this happens during a copy operation:

Also, we should understand the method we are using to obtain our PID controller's quat error, and get rid of the "dead zone":