Reverted DOF index check.. I think it's actually OK.
Reverted DOF index check.. I think it's actually OK.

personalrobotics/trajopt,personalrobotics/trajopt,personalrobotics/trajopt,personalrobotics/trajopt,personalrobotics/trajopt,personalrobotics/trajopt,personalrobotics/trajopt