Orientation correction from joystick command

In the current implementation of the orientation controller for the 6-DOF diffusion policy execution, the correction from the joystick is not working properly. The command from the joystick is added to the current quaternions, but it does not really adjust the trajectory.

The implementation should be rethought. For example, it can freeze the current quaternion if the angular velocity from joystick is received and make a correction or clever gain handling.

Optionally allow override of the gripper command from the spacemouse.