Changes made to test trajectory generation with MotionState msg. The
generations is okay, but setting up Reflexxes to work properly with the
Corobot still needs to be done.