Commanded motion optimization to reduce residual vibration