Fixed issue in allocating controllers.
Added default COM reference to be coincident to the initial COM read from the robot