[Servo] Use velocity scaling properly in Cartesian and pose tracking commands

[Servo] Use velocity scaling properly in Cartesian and pose tracking commands #7473