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

Sparse programming is an important tool in robotics, for example in real-time sparse inverse kinematic control with a minimum number of active joints, or autonomous Cartesian goal selection. However, current approaches are limited to real-time control without consideration of the underlying non-linear problem. This prevents the application to non-linear problems like inverse kinematic planning while the robot simultaneously and autonomously chooses from a set of potential end-effector goal positions. Instead, kinematic reachability approximations are used while the robot's whole body motion is considered separately. This can lead to infeasible goals. Furthermore, the sparse constraints are not prioritized for intuitive problem formulation. Lastly, the computational effort of standard sparse solvers is cubically dependent on the number of constraints which prevents real-time control in the presence of a large number of possible goals. In this work, we develop a non-linear solver for sparse hierarchical non-linear programming. Sparse non-linear constraints for autonomous goal selection can be formulated on any priority level, which enables hierarchical decision making capabilities. The solver scales linearly in the number of constraints. This facilitates efficient robot sparse hierarchical inverse kinematic planning and real-time control with simultaneous and autonomous goal selection from a high number of possible goal positions without any reachability approximations.