Agility for Underwater Floating Manipulation Task and Subsystem Priority based Control Strategy