Dynamically-Consistent Trajectory Optimization for Legged Robots via Contact Point Decomposition

Abstract: To generate reliable motion for legged robots through trajectory optimization, it is crucial to simultaneously compute the robot's path and contact sequence, as well as accurately consider the dynamics in the problem formulation. In this paper, we present a phase-based trajectory optimization that ensures the feasibility of translational dynamics and friction cone constraints throughout the entire trajectory. Specifically, our approach leverages the superposition properties of linear differential equations to decouple the translational dynamics for each contact point, which operates under different phase sequences. Furthermore, we utilize the differentiation matrix of B{\'e}zier polynomials to derive an analytical relationship between the robot's position and force, thereby ensuring the consistent satisfaction of translational dynamics. Additionally, by exploiting the convex closure property of B{\'e}zier polynomials, our method ensures compliance with friction cone constraints. Using the aforementioned approach, the proposed trajectory optimization framework can generate dynamically reliable motions with various gait sequences for legged robots. We validate our framework using a quadruped robot model, focusing on the feasibility of dynamics and motion generation.

• The paper introduces a phase-based trajectory optimization framework that uses contact point decomposition and Bézier polynomial parameterizations to ensure continuous translational dynamics and friction cone compliance.
• The method decouples legged robot forces via convex hull properties, resulting in zero translational dynamics and friction violations across various terrains.
• Utilizing SO(3) tangent space for angular dynamics, the framework achieves improved tracking accuracy and robustness over baselines like TOWR.

Dynamically-Consistent Trajectory Optimization for Legged Robots via Contact Point Decomposition

Introduction and Motivation

Trajectory optimization (TO) for legged robots requires simultaneous computation of body motion, contact sequence, and dynamic feasibility. Traditional approaches, such as Mixed Integer Programming (MIP) and Contact Implicit Trajectory Optimization (CITO), either suffer from exponential complexity or local minima due to hybrid contact modes. Phase-Based Trajectory Optimization (PBTO) offers scalability by parameterizing contact sequences as repeated stance and swing phases, but prior PBTO methods lack guarantees of dynamic feasibility between collocation nodes, especially for short phases. This paper introduces a PBTO framework that enforces translational dynamics and friction cone constraints continuously throughout the trajectory by leveraging contact point decomposition and the analytical properties of Bézier polynomials.

Figure 1: Overview of the proposed method, illustrating the decomposition of contact forces and trajectory parameterization via Bézier polynomials.

Methodology

Contact Point Decomposition and Bézier Polynomial Parameterization

The robot is modeled as a single rigid body (SRB) with massless point feet. The translational dynamics are linear and thus amenable to superposition: the effect of each contact point is decoupled and parameterized independently. Each contact force and foot trajectory is represented as a Bézier polynomial, whose control points are optimization variables. The analytical relationship between position and force is derived using the differentiation matrix of Bézier polynomials, enabling direct enforcement of translational dynamics at all points in the trajectory, not just at collocation nodes.

The convex hull property of Bézier polynomials is exploited to guarantee that the entire force trajectory remains within the friction cone if all control points satisfy the cone constraints. This is reformulated as a friction pyramid for numerical stability.

Angular Dynamics and SO(3) Representation

Angular dynamics, being nonlinear, are discretized and enforced at fixed nodes. Unlike previous methods that use Euler angles, the orientation is parameterized in the tangent space of SO(3), mitigating singularity issues and improving optimization robustness.

Optimization Formulation

The optimization variables include Bézier control points for forces and foot positions, phase durations, discretized orientations, angular velocities, and initial states of position segments. The objective function is a weighted sum of terms penalizing deviation from nominal body and foot trajectories, as well as regularization of foot velocity and acceleration. Constraints enforce final state conditions, continuity, foot-terrain contact, kinematic limits, friction cone satisfaction, and angular dynamics.

Sequential Quadratic Programming (SQP) with analytical gradients and Gauss-Newton Hessian approximation is used to solve the problem. Convergence is determined by relative changes in objective and constraint violation metrics.

Experimental Results

Feasibility and Robustness Across Terrains

The framework was validated on a quadruped robot traversing various terrains: flat plane, block, stairs, and inclined chimney. For each terrain, 300 randomized trials were conducted, sampling initial/final velocities, yaw angles, friction coefficients, and gait durations.

Figure 2: The proposed TO framework generated motion on various terrains, including flat, stepped, and inclined surfaces.

The proposed method achieved zero translational dynamics (TD) and friction cone (FC) violations in all successful optimizations, as shown in Table 1. In contrast, the baseline (TOWR) exhibited significant violations, especially in TD and FC, due to enforcement only at collocation nodes. Angular dynamics (AD) violations were also reduced compared to the baseline, attributed to the SO(3) representation.

Dynamics Consistency

Figure 3: Gravity-compensated linear momentum derivative and total GRF along the z-axis for trajectories generated by the proposed method and the baseline.

The proposed method maintains dynamic consistency throughout the trajectory, whereas the baseline only satisfies dynamics at discrete nodes, leading to physically infeasible segments.

Friction Cone Satisfaction

Figure 4: GRF profile projected onto the x-z plane and x/z force ratio over time for the front right leg, comparing the proposed method and the baseline.

The control points of the Bézier polynomial in the proposed method ensure the entire force trajectory remains within the friction pyramid, eliminating instances of negative GRF or friction cone violation observed in the baseline.

Trajectory Tracking Performance

The reference trajectories generated by the proposed framework were tracked using a model predictive controller (MPC) on a quadruped robot. Across 50 randomized trials, the proposed method yielded lower mean and standard deviation in both position and orientation tracking errors compared to the baseline, indicating improved suitability for control.

Metric	Ours (Mean ± Std)	Baseline (Mean ± Std)
X Position Error [m]	0.132 (0.036)	0.165 (0.052)
Y Position Error [m]	0.048 (0.031)	0.069 (0.035)
Z Position Error [m]	0.016 (0.002)	0.024 (0.002)
Orientation Error [rad]	0.061 (0.041)	0.104 (0.046)

Trade-offs and Limitations

The strict enforcement of dynamic and friction cone feasibility increases the computational burden and, in some scenarios, the rate of optimization failure, particularly on complex terrains. Relaxing the friction cone constraint to node-based enforcement reduces computation time and failure rate, but at the cost of increased feasibility violations. The method's scalability is determined by the number of phases and control points, which must be balanced against solver performance.

Implications and Future Directions

The proposed framework advances trajectory optimization for legged robots by guaranteeing dynamic consistency and friction cone satisfaction throughout the trajectory, not just at collocation nodes. This is critical for generating reference motions for model-based and learning-based controllers in scenarios where physical feasibility is paramount. The analytical integration and convex hull enforcement via Bézier polynomials represent a significant methodological improvement over collocation-based approaches.

Future work will focus on integrating the framework as a reference generator for MPC and RL-based controllers, extending to more complex robot morphologies, and further improving solver efficiency. The approach is well-suited for applications requiring robust locomotion over diverse and challenging terrains.

Conclusion

This paper presents a dynamically-consistent PBTO framework for legged robots, leveraging contact point decomposition and Bézier polynomial properties to enforce translational dynamics and friction cone constraints continuously. The method demonstrates superior dynamic feasibility and tracking performance compared to collocation-based baselines, with practical implications for robust motion planning and control in legged robotics. The analytical and convex properties of the formulation provide a foundation for future developments in trajectory optimization and controller design for dynamic locomotion.