An Efficient Numerical Function Optimization Framework for Constrained Nonlinear Robotic Problems

TL;DR

The paper tackles real-time constrained nonlinear optimization in robotics where analytical problem representations and Hessians are impractical. It introduces a hierarchical, nullspace-projection framework that uses first-order gradients and active-constraint Jacobians to preserve feasibility while reducing the objective, avoiding Hessian computations. A modular C++ implementation (ENFORCpp) provides a ProblemDescription interface with four user-defined functions for cost and constraints plus an interim update, along with subroutines for equality, inequality, and cost optimization. The approach is validated on five problems, including a 3-link arm and a humanoid posture task, demonstrating micro- to millisecond-level solve times and online applicability for trajectory and control input optimization.

Abstract

This paper presents a numerical function optimization framework designed for constrained optimization problems in robotics. The tool is designed with real-time considerations and is suitable for online trajectory and control input optimization problems. The proposed framework does not require any analytical representation of the problem and works with constrained block-box optimization functions. The method combines first-order gradient-based line search algorithms with constraint prioritization through nullspace projections onto constraint Jacobian space. The tool is implemented in C++ and provided online for community use, along with some numerical and robotic example implementations presented in this paper.

Figures (3)

• Figure 1: Inequality constraint activation conditions where the red dot, yellow star, and red region represent the initial condition, desired position, and constraint, respectively. The left problem never reaches the activation point. The middle problem starts from the zero crossing point, but the cost function gradient is already in the same direction as the constraint gradient. Hence, the cost function iteration will not exceed the constraint. The right problem also starts from the zero crossing point, and the cost function and constraint gradients are in reverse directions. The constraint is activated. The cost function gradient should be projected onto the nullspace of the constraint gradient, which results in a zero vector in this case. As a result $x$ remains at $x_{0}$.
• Figure 2: Left: A three-link planar robot arm system, where the red dot and yellow star represent the initial condition and the desired position, respectively. Right: The result of the optimization problem where the arm folds onto itself to minimize the static torque requirement while satisfying the equality constraint.
• Figure 3: The snapshots of the optimized trajectory.