Hierarchy optimal control & shared autonomy

Summary
Publication submitted to peerreview journal