A control system for a minimally invasive surgical system. In one aspect the control system is a distributed system. A control and transform processor receives data from a master arm controller, an instrument controller, an imaging system controller, and a guide tube controller and distributes data received from one controller to the other controllers. The other controllers use the received data, along with received optimization goals, to control associated slave arms in a distributed but coordinated way. In another aspect, the control system is centralized, in which a motion coordinator receives master inputs, sensor inputs from the slave arms, and optimization inputs. The motion coordinator uses the received inputs to output control signals to an instrument, an imaging system, and a guide tube controller.