Sparse Hierarchical Non-Linear Programming for Inverse Kinematic Planning and Control with Autonomous Goal Selection