Integrated Hierarchical Decision-Making in Inverse Kinematic Planning and Control

This work presents a novel and efficient non-linear programming framework that tightly integrates hierarchical decision-making with inverse kinematic planning and control. Decision-making plays a central role in many aspects of robotics, from sparse inverse kinematic control with a minimal number of joints, to inverse kinematic planning while simultaneously selecting a discrete end-effector location from multiple candidates. Current approaches often rely on heavy computations using mixed-integer non-linear programming, separate decision-making from inverse kinematics (some times approximated by reachability methods), or employ efficient but less accurate $\ell_1$-norm formulations of linear sparse programming, without addressing the underlying non-linear problem formulations. In contrast, the proposed sparse hierarchical non-linear programming solver is efficient, versatile, and accurate by exploiting sparse hierarchical structure and leveraging the rarely used $\ell_0$-norm in robotics. The solver efficiently addresses complex non-linear hierarchical decision-making problems, such as inverse kinematic planning with simultaneous prioritized selection of end-effector locations from a large set of candidates, or inverse kinematic control with simultaneous selection of bi-manual grasp locations on a randomly rotated box.

💡 Research Summary

The paper introduces a novel optimization framework called Sparse Hierarchical Non‑Linear Programming (SH‑NLP) and a dedicated solver named Sequential Sparse Hierarchical Quadratic Programming (S‑SHQP) to address the combined problem of sparse inverse‑kinematics (IK) control and discrete decision‑making in robotics. Traditional approaches either rely on Mixed‑Integer Non‑Linear Programming (MINLP), which is computationally prohibitive, or on ℓ₁‑norm relaxations that cannot guarantee true sparsity. By directly employing the ℓ₀‑norm, the authors achieve genuine sparsity, but because the ℓ₀‑norm is combinatorial, they approximate it with a logarithmic surrogate, turning the original combinatorial problem into a continuous non‑linear program while preserving the hierarchical structure of tasks.

The SH‑NLP formulation introduces slack variables for each task level and penalizes the number of non‑zero slacks using the log‑sum surrogate, with a small constant ξ for numerical stability. At each iteration, the non‑linear problem is linearized around the current state xₖ, yielding a Sparse Hierarchical Quadratic Program (SH‑QP). The SH‑QP is solved by a custom interior‑point method (N‑QP) that exploits the special block‑sparse structure: active constraints from higher priority levels are projected out using a null‑space basis N_{l‑1}, reducing the dimensionality of the sub‑problem. A hierarchical Lagrangian Hessian H_l captures second‑order information for the current level, allowing a Newton step that improves convergence, especially in singular or near‑infeasible tasks. A trust‑region constraint ensures the local quadratic model remains valid, and a Hierarchical Step‑Filter (HSF) evaluates feasibility and optimality of each proposed step.

Two operational modes are defined. In planning mode (SHIK‑P), the HSF decides whether to accept a step, adjusts the trust‑region radius, and iterates until convergence, after which inactive constraints are removed and weights ω are fixed for lower levels. In control mode (SHIK‑C), each step is directly accepted with a constant trust‑region, enabling real‑time closed‑loop control at millisecond rates.

Experimental validation covers four scenarios: (a) sequential pick‑and‑place with a single xArm6 robot; (b) a Unitree G1 robot simultaneously selecting among 200 candidate foot locations and 200 hand locations, solving 10 postures via 763 SH‑QP solves in 2.12 s; (c) cooperative cleaning of a conveyor belt using four xArm6 arms; (d) a Unitree G1 placing its hands on two faces of a randomly rotated box. In the most demanding real‑time tests, SHIK‑C with 100 candidate locations runs at 1.5 ms per control cycle, outperforming PIQP (2.2 ms) and MOSEK (8.3 ms). Moreover, the ℓ₀‑based selection eliminates redundant choices that appear with ℓ₁ relaxations, improving task success rates and energy efficiency.

The contributions are: (1) a true ℓ₀‑norm hierarchical formulation for robotic decision‑making; (2) a scalable solver that leverages null‑space projection and hierarchical Hessians; (3) integration of trust‑region and step‑filter mechanisms for global convergence; (4) extensive real‑robot experiments demonstrating both planning and high‑frequency control capabilities. Limitations include sensitivity to the surrogate parameters (ξ, ω) and scalability concerns for problems with tens of thousands of candidates, which the authors suggest addressing via automatic parameter tuning, parallelization, and adaptive trust‑region strategies in future work.