1. Forgot to check in the motor position class.
2. Commenting out the code to do quantization of the joint position/velocity