Fixed small errors. This version now has an error that says the goal is invalid because the quaternion's z axis is not close to vertical. I don't know why.