A system for controlling a vehicle having an autonomous mode and a semi-autonomous mode includes one or more processors and a memory in communication with the one or more processors. The memory stores a command generating module and a transmission module. The command generating module causes the one or more processors to generate, in response to an input, at least one control signal for controlling the vehicle by an envelope control system. The envelope control system utilizes a common control scheme for both the semi-autonomous mode and the autonomous mode, wherein the input is a driver input when the vehicle is in the semi-autonomous mode and the input is a pseudo-driver input when the vehicle is in the autonomous mode. The transmission module causes the one or more processors to transmit the at least one control signal to a vehicle motion controller, wherein the vehicle motion controller controls the movement of the vehicle.