CMC-Opt: Constraint Manifold with Corners for Inequality-Constrained Optimization

Abstract: We introduce a manifold-based framework for addressing optimization problems with equality and inequality constraints found in robotics. Our approach transforms the original problem into an unconstrained optimization problem directly on the constrained state space. To achieve this, we introduce ``constraint manifolds with corners" to represent the state space satisfying mixed nonlinear equality and inequality constraints. We further extend manifold optimization algorithms to operate on this new topological structure. We demonstrate the power and robustness of our framework in the context of a large-scale kinodynamic planning problem, successfully generating dynamically feasible trajectories where standard methods fail.

• The paper introduces CMC-Opt, a novel framework that integrates both equality and inequality constraints into manifold optimization for robotics.
• It leverages factor graph decomposition and a Riemannian gradient descent approach to compute feasible descent directions on challenging non-smooth constraint sets.
• Empirical evaluations on quadruped kinodynamic planning demonstrate significant state-space reduction and exact constraint satisfaction, ensuring physically feasible trajectories.

CMC-Opt: Constraint Manifolds with Corners for Nonlinear Inequality-Constrained Optimization

Introduction and Motivation

Optimization with nonlinear equality and inequality constraints is ubiquitous in advanced robotics, notably in kinodynamic trajectory optimization and control. While classical approaches—such as penalty, augmented Lagrangian, and sequential quadratic programming—operate in the ambient space and manage constraints with cost augmentation or KKT-driven methods, these are afflicted by scaling issues, poor conditioning, and convergence reliability, especially when hard nonlinear constraints define the robotic system's state and actuation limits.

Manifold optimization addresses equality constraints by transforming the problem into unconstrained minimization on a lower-dimensional manifold, improving both computational tractability and numerical stability. However, standard manifold optimization is fundamentally incompatible with inequality constraints, which induce non-smooth "boundaries" and high-order "corners" in state space, violating the local Euclidean property that manifold methods exploit.

This work introduces a novel theoretical and algorithmic framework—Constraint Manifold with Corners Optimization (CMC-Opt)—that seamlessly integrates nonlinear equality and inequality constraints, allowing optimization to be conducted directly on the feasible set. The methodology bridges differential geometry, factor graph decomposition, and Riemannian optimization to enable principled, scalable solutions to high-dimensional constrained robotics tasks.

Constraint Manifolds with Corners: Theory

Given a general constrained optimization problem with nonlinear equality and inequality constraints, CMC-Opt employs a factor graph abstraction to analyze and localize constraint-connected variable subsets. For a variable vector $X \in \mathbb{R}^N$, equality constraints $h(X)=0$ and inequality constraints $g(X)\geq0$, the feasible state space is:

$$M = \{ X \in \mathbb{R}^N : h(X) = 0,~g(X) \geq 0 \}$$

Unlike smooth manifolds, $M$ exhibits boundaries (single active inequality constraint) and corners (multiple active constraints). At any feasible point, the tangent space $T_X M$ is the set of allowable velocities under the linearized constraint system, forming a convex cone at corners, rather than a subspace. CMC-Opt leverages local parameterizations of these corners via model spaces $H_m^n$ (non-negative orthants) and ensures full rank of local Jacobians for regularity.

The abstraction is depicted in factor graph form, highlighting the restructured constraint relations and enabling modular decomposition.

Figure 1: Factor graph representations of constraints before and after transformation to optimization on constraint manifolds with corners.

CMC-Opt generalizes the tangent space formalism from regular manifolds to manifolds with corners, encompassing both unconstrained (interior) regions and the complex geometry at boundaries and intersections of active sets.

A key technical contribution is the intrinsic extension of Riemannian gradient calculation: the feasible direction for descent is determined by solving a QP in the local tangent cone, ensuring all constraint qualifications are rigorously respected.

CMC-Opt also introduces a retraction operation—projection back onto $M$—using Euclidean metric projection, ensuring iterates remain feasible following each step.

Figure 2: (a) Example constraint manifolds with corners and active set coloring. (b) Tangent space structure at various points, distinguishing between full, partial, and restricted freedom. (c) Illustration of retraction after a tangent step.

Algorithmic Framework: Riemannian Descent on CMCs

CMC-Opt implements a generalized manifold optimization procedure based on Riemannian gradient descent principles. For each optimization iteration:

1. The cost function's differential is calculated within the tangent cone at the current point.
2. The feasible descent direction is computed via a QP, maximizing decrease in cost under the inequality-induced constraints.
3. A step is taken along the tangent direction, followed by projection (retraction) onto $M$.
4. Iteration proceeds until convergence is achieved.

A didactic application is shown by optimizing the gravitational potential of a particle constrained to a "half-sphere" (sphere plus $z\geq0$ constraint), visualizing the optimization trajectory respecting the curved boundary.

Figure 3: Riemannian gradient descent on a half-sphere, demonstrating boundary-respecting descent and convergence.

Empirical Evaluation: Quadruped Kinodynamic Planning

CMC-Opt is evaluated on a challenging kinodynamic trajectory planning task for a 13-link quadruped, consisting of four hybrid contact phases. The state includes detailed link poses, twists, joint angles, velocities, torques, and explicit contact force variables—yielding a highly over-constrained and stiff system (over 32,000 variables in baselines).

Constraints encompass:

• Rigid-body and kinematic chain dynamics (equalities),
• Linking/contact point holonomy,
• Collision avoidance, joint/actuator bounds, and Coulomb friction (inequalities).

The cost function aggregates torque effort, trajectory smoothness, target accuracy, and hard collocation. Initial guesses are highly infeasible.

CMC-Opt is benchmarked against:

• Penalty and augmented Lagrangian methods,
• Trust-region SQP,
• CM-Opt (equality-only manifold method, with inequalities as penalties).

The results show CMC-Opt achieves strict constraint satisfaction (zero infeasibility), a much lower final cost, and drastically reduces the state-space dimension from 32,194 to 2,260. The generated quadruped trajectory is physically feasible and smooth, unlike penalty/SQP baselines that violate discretized dynamics (collocation leakage).

Figure 4: Comparison of optimized quadruped jumping trajectories: CMC-Opt (bottom) vs. penalty method (top). Only CMC-Opt yields dynamically-feasible, natural motion.

Implications and Future Directions

CMC-Opt fundamentally advances robotic trajectory and control optimization by enabling exact satisfaction of nonlinear constraints—including those producing facet boundaries and high-order corners—within a rigorous geometric framework. By projecting updates onto lower-dimensional, constraint-manifold tangent cones, the search space for feasible solutions is vastly reduced, often by orders of magnitude.

Notably, CMC-Opt decouples hard constraint management from cost minimization, transforming a multi-objective, ill-conditioned optimization into a principled, feasible-space search.

The practical implementation leverages a manifold-variant of Levenberg-Marquardt, incorporating Gauss-Newton approximations and trust regions, thus maintaining competitive robustness and convergence speeds in large-scale settings typical of articulated robotic platforms.

Current limitations remain in handling cross-time-step or non-local constraints, which, if incorporated directly, can render projection (retraction) computationally intractable. These are currently relaxed via penalties, but future research may seek hierarchical or locally-adaptive retraction strategies, distributed optimization, or hybrid penalty-manifold schemes for scalability.

The theoretical constructs of CMC-Opt further open avenues for sampling-based planning, learning-based policy search, and control in complex environments where feasible state sets possess intricate boundary geometry.

Conclusion

CMC-Opt presents a mathematically sound and practically efficient approach to nonlinear inequality-constrained optimization, particularly within high-dimensional, kinodynamically-constrained robotic systems. By extending manifold optimization to non-smooth feasible sets characterized by boundaries and corners, and integrating this within a factor graph and retraction-based computational framework, the method achieves exact constraint satisfaction and state-of-the-art performance on complex trajectory optimization problems. CMC-Opt provides both a rigorous foundation for future algorithmic research and a potent practical tool for advanced robotics applications.