Prioritized optimization for task-space control

We introduce an optimization framework called prioritized optimization control, in which a nested sequence of objectives are optimized so as not to conflict with higher-priority objectives. We focus on the case of quadratic objectives and derive an efficient recursive solver for this case. We show how task-space control can be formulated in this framework, and demonstrate the technique on three sample control problems. The proposed formulation supports acceleration, torque, and bilateral force constraints, while simplifying reasoning about task-space control. This scheme unifies prioritized task-space and optimization-based control. Our method computes control torques for all presented examples in real-time.