Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

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

Kai Pfeiffer

TL;DR

The paper addresses real-time inverse kinematic planning and control with autonomous goal selection by formulating Sparse Hierarchical Non-Linear Programming (SH-NLP) and solving it with Sequential Sparse Hierarchical Quadratic Programming (S-SHQP). It introduces a log-sum surrogate to approximate the $\ell_0$ sparsity and develops an interior-point SHQP solver that scales linearly with the number of sparse constraints, enabling AGS across large goal sets. The approach supports both planning (SHIK-P) and control (SHIK-C) with autonomous goal selection on complex robots (e.g., HRP-2Kai, UR3e), validated against state-of-the-art solvers and demonstrated in real-time scenarios. This work significantly advances real-time, sparsity-aware, non-linear hierarchical optimization in robotics, reducing reliance on reachability approximations and enabling scalable decision-making with many potential goals.

Abstract

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.

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

TL;DR

The paper addresses real-time inverse kinematic planning and control with autonomous goal selection by formulating Sparse Hierarchical Non-Linear Programming (SH-NLP) and solving it with Sequential Sparse Hierarchical Quadratic Programming (S-SHQP). It introduces a log-sum surrogate to approximate the sparsity and develops an interior-point SHQP solver that scales linearly with the number of sparse constraints, enabling AGS across large goal sets. The approach supports both planning (SHIK-P) and control (SHIK-C) with autonomous goal selection on complex robots (e.g., HRP-2Kai, UR3e), validated against state-of-the-art solvers and demonstrated in real-time scenarios. This work significantly advances real-time, sparsity-aware, non-linear hierarchical optimization in robotics, reducing reliance on reachability approximations and enabling scalable decision-making with many potential goals.

Abstract

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.

Paper Structure

This paper contains 14 sections, 1 theorem, 15 equations, 6 figures, 4 tables.

Table of Contents

  1. Introduction
  2. Sparse Hierarchical Non-Linear Programming
  3. Sequential Sparse Hierarchical Quadratic Programming
  4. Planning
  5. Control
  6. Selection constraints
  7. An interior-point method for SHQP
  8. Evaluation
  9. Optimization test functions
  10. HRP-2Kai SHIK-P: autonomous goal selection
  11. UR3e SHIK-C: simultaneous SHIK-C and AGS
  12. HRP-2Kai SHIK-C: Continuous selection
  13. Conclusion
  14. Activity of auxiliary constraints

Key Result

Theorem 1

$\hat{v}_{\mathbb{C}_l}$ is strictly upper or lower bounded at $\hat{t}_{\mathbb{C}_l}$ or $-\hat{t}_{\mathbb{C}_l}$ for $\omega_{\mathbb{C}_l} > 0$ and the infeasible case ${\hat{v}}_{\mathbb{C}_l}\neq 0$. Otherwise $\hat{t}_{\mathbb{C}_l} = {\hat{v}}_{\mathbb{C}_l} = 0$.

Figures (6)

  • Figure 1: A symbolic overview of the sequential sparse hierarchical quadratic programming (S-SHQP) with trust region and hierarchical step-filter (HSF) pfeiffer2024 based on the SQP step-filter fletcher2002b to solve sparse hierarchical non-linear programs \ref{['eq:hnlsp']} with $p$ levels. Our contributions are marked in blue.
  • Figure 1: Non-linear test functions: optimal slacks $v^*$ and number of S-SHQP iterations (Iter.) per priority level for a \ref{['eq:hnlsp']} with $p=10$ and $n=10$. $\ell$ indicates the norm (0 or 2) of the constraints of each level $l$. The hierarchy is composed of disk, Rosenbrock (Ros.), McCormick (McC.) and regularization (Reg.) equality (eq.) and inequality (ineq.) constraints. Zero constraints are marked in bold.
  • Figure 2: HRP-2Kai SHIK-P with AGS: resulting robot posture.
  • Figure 3: UR3e SHIK-C: tracking error to target 1 ($e_1$) and 2 ($e_2$), $\mathcal{N}$QP.
  • Figure 4: UR3e SHIK-C: joint angles, $\mathcal{N}$QP.
  • ...and 1 more figures

Theorems & Definitions (2)

  • Theorem 1
  • proof