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

Sangmin Kim, Hajun Kim, Gijeong Kim, Min-Gyu Kim, Hae-Won Park
Dynamic Robot Control & Design Laboratory (DRCD), KAIST
IEEE Robotics and Automation Letters, 10(12), 13161–13168, 2025
Code arXiv IEEE BibTeX

Plane

Block · 0.5 m

Stairs

ChimneyLR · 45°

Our framework generates dynamically consistent motions across flat ground, a 0.5 m block, stairs, and 45° inclined walls.

Abstract

To generate reliable motion for legged robots through trajectory optimization, it is crucial to compute the robot path and contact sequence while accurately considering dynamics. We present a phase-based trajectory optimization framework that ensures translational dynamics and friction-cone feasibility throughout the entire trajectory. The method decomposes translational dynamics for each contact point using the superposition property of linear differential equations, derives an analytical relationship between robot position and force using Bézier differentiation matrices, and exploits the convex-hull property of Bézier polynomials to satisfy friction-cone constraints. We validate the framework with a quadruped model across multiple terrains and gait sequences.

Method

Overview of contact point decomposition and phase-based trajectory optimization

Each contact point is optimized over its own stance and swing phases, then recombined into a whole-body trajectory.

  • Contact point decomposition. The effects of individual contact forces are separated into multiple-phase problems using the superposition property of translational dynamics.
  • Analytical dynamics. Bézier differentiation matrices relate robot position and contact force analytically, satisfying translational dynamics throughout each phase.
  • Continuous friction feasibility. Constraining Bézier control points inside the friction pyramid keeps the complete force profile inside it.

Results

Various Motions

Dynamic Motion

Backflip

Standing Motion

Translational dynamics

Analytical integration makes the gravity-compensated linear momentum derivative match the total ground-reaction force continuously. The proposed formulation yields zero translational-dynamics violation across the evaluated terrains.

Comparison of translational dynamics feasibility for the proposed method and baseline
Ground reaction force profiles compared against the friction pyramid

Friction-cone feasibility

The convex-hull property turns finite control-point constraints into continuous-time friction feasibility. Friction-cone violation remains near zero, up to numerical precision.

Trajectory tracking

Across 50 randomized MPC simulations, trajectories generated by our method reduced the mean tracking error in every position and orientation axis compared with the baseline. The videos below show each TO-generated reference and its corresponding MPC tracking result.

MetricOursBaseline
Position X0.132 m0.165 m
Position Z0.016 m0.024 m
Orientation X0.061 rad0.104 rad

Proposed Method

TO-Generated Reference

MPC Tracking

Baseline (TOWR; Winkler et al., 2018)

TO-Generated Reference

MPC Tracking

Baseline reference: A. W. Winkler, C. D. Bellicoso, M. Hutter, and J. Buchli, “Gait and Trajectory Optimization for Legged Systems Through Phase-Based End-Effector Parameterization,” IEEE Robotics and Automation Letters, 3(3), 1560–1567, 2018. Paper · TOWR code

Video Presentation

Four-minute overview of the motivation, formulation, and experimental validation.

BibTeX

@article{kim2025dynamically,
  title   = {Dynamically-Consistent Trajectory Optimization for Legged Robots via Contact Point Decomposition},
  author  = {Kim, Sangmin and Kim, Hajun and Kim, Gijeong and Kim, Min-Gyu and Park, Hae-Won},
  journal = {IEEE Robotics and Automation Letters},
  year    = {2025},
  volume  = {10},
  number  = {12},
  pages   = {13161--13168},
  doi     = {10.1109/LRA.2025.3628199}
}