An Integrated Framework For Planning And Control Of Semi-Autonomous Vehicles