Contact-Implicit Whole-Body Trajectory Optimization for Dynamic Humanoid Locomotion on Parameterizable Uneven Terrain

Roberto Mauceri; Stefano Dafarra; Giulio Romualdi; Marco Gabiccini; Daniele Pucci

cs.RONov 3, 2026

Contact-Implicit Whole-Body Trajectory Optimization for Dynamic Humanoid Locomotion on Parameterizable Uneven Terrain

Authors: Roberto Mauceri, Stefano Dafarra, Giulio Romualdi, Marco Gabiccini, Daniele Pucci

Organizations: Artificial and Mechanical Intelligence, Istituto Italiano di Tecnologia, 16163 Genoa, Italy. · Dept. of Computer Science, University of Manchester, Manchester M13 9PL, U.K. · Dept. of Civil and Industrial Engineering, University of Pisa, 56126 Pisa, Italy.

Abstract

This letter presents a trajectory planner for humanoid robots to perform dynamic motions on complex, parameterizable terrains using a contact-implicit whole-body optimization framework. The planner generates feasible movements without predefined contact sequences, considering the robot's full dynamics and kinematics. To plan bipedal locomotion on steps, ramps, and other terrains, we propose a mathematical model to approximate ground geometry with a smooth surface. The rigid interaction between the robot and the ground is modeled via novel contact constraints, extending the domain of applicability from flat and horizontal surfaces to any arbitrary smooth surface. Effectiveness is demonstrated using nonlinear programming to plan motions on various terrain topologies. We also explore dynamic trajectory generation by embedding high-level requirements in the cost function and the initial guess. Preliminary feasibility tests were conducted over non-coplanar terrains, both in simulation and on the real robot ErgoCub.

Figures & tables

	A	B	C	D	E	F	G	H	I
Ace17 [ 12 ]		✓			CD	MICP	✓
Win18 [ 19 ]		✓	✓	✓	CD	NLP		✓
Mas20 [ 11 ]		✓		✓	WBD	FDDP	✓	✓
Ton20 [ 13 ]		✓			CoM	MICP	✓	✓
Jen22 [ 15 ]		✓	✓		GIA	NLP	✓		✓
Our Work	✓	✓	✓	✓	WBD	NLP		✓	✓

TABLE I: Comparison of optimizers for planning on uneven terrain.

Fig. 1: The robot ErgoCub performing a step-up trajectory (open loop).

Fig. 2: Surface with a torus-like shape: $f(x,\,y)=\exp\left\{-g(x,\,y)^{s}\right\}$ with $g(x,\,y)=(x^{2}+y^{2}-2)^{2}$ and $s=4$ . The blue curve is the geometric locus of points satisfying the implicit function $g(\bm{x})=0$ , the red curves are the geometric locus of points satisfying the implicit function $g(\bm{x})=1$ .

Fig. 3: Time evolution of the contact condition (up) and the nine components of no-slip condition (down) for a single contact point during walking on undulating terrain ( Fig. 4 (c)), with relaxation intervals of $[0,\,0.001]$ $\mathrm{N}\text{\,}\mathrm{m}$ and $[-0.001,\,+0.001]$ $\mathrm{N}\text{\,}\mathrm{m}\text{\,}{\mathrm{s}}^{-1}$ respectively. Shaded areas indicate contact phases. The point is considered to be in contact when its vertical distance from the ground is below $1\text{\,}\mathrm{mm}$ .

Panel of Figure 5 — Fig. 4: Examples of generated trajectories: (a) inclined platform jumps, (b) crater walk, (c) undulating terrain walk, and (d) challenging terrain walk.

	T [ $\mathrm{s}$ ]	N	MTM	CIP	CV	CT [ $\mathrm{s}$ ]	I
(a)	$5.0$	$60$			$1.13e{-4}\text{\,}$	$75.8$	$350$
(b)	$6.0$	$60$	✓	✓	$3.57e{-4}\text{\,}$	$55.3$	$250$
(c)	$5.0$	$180$	✓	✓	$1.42e{-3}\text{\,}$	$186.6$	$250$
(d)	$9.0$	$180$	✓	✓	$4.26e{-3}\text{\,}$	$173.3$	$250$

TABLE II: Trajectory generation: example data ( Fig. 4 ).

Contact-Implicit Trajectory Optimization

See all 9 papers

Robust bipedal locomotion on flowable slopes via foot-driven terrain manipulation
Jul 13, 2026Deniz Kerimoglu, Junnosuke Kamohara, Jiyeon Maeng +4Humanoid Robot Locomotion Granular Terrain Locomotion
Department of Mechanical Engineering, Georgia Institute of Technology, Atlanta, GA 30332, USA · School of Physics, Georgia Institute of Technology, Atlanta, GA 30332, USA · Department of Electrical and Computer Engineering, Northeastern University, Boston, MA 02115, USA
Multi-Terrain Mastery: A Comprehensive Controller for Bipedal Locomotion
Sep 27, 2026Oluwami Dosunmu-Ogunbi, Aayushi ShrivastavaRobot Locomotion Legged Robotics
Mechanical Engineering Department Ohio Northern University Ada, United States of America · Mechanical Engineering Department University of California, Berkeley Berkeley, United States of America
Perceptive Variable-Timing Footstep Planning for Humanoid Locomotion on Disconnected Footholds
Mar 8, 2026Zhaoyang Xiang, Upama Pant, Ayonga HereidHumanoid Robot Locomotion Terrain-Aware Robot Locomotion
Mechanical and Aerospace Engineering, The Ohio State University, Columbus, OH 43210, USA.

Jul 13, 2026J/K move · Enter open · S save

Robust bipedal locomotion on flowable slopes via foot-driven terrain manipulation

Deniz Kerimoglu, Junnosuke Kamohara, Jiyeon Maeng, Ziwon Yoon+3

Department of Mechanical Engineering, Georgia Institute of Technology, Atlanta, GA 30332, USA · School of Physics, Georgia Institute of Technology, Atlanta, GA 30332, USA · Department of Electrical and Computer Engineering, Northeastern University, Boston, MA 02115, USA

Humanoid Robot Locomotion Granular Terrain Locomotion Terrain-Aware Robot Locomotion Robot Locomotion Bipedal Locomotion

Open paper AskPDF arXiv

Bipedal robots are challenging to control because they operate close to instability, where small variations in foot-terrain contact can rapidly destabilize locomotion. On rigid terrain, bipedal robots mitigate this fragility by using well-established contact mechanics and control strategies. On flowable surfaces such as granular slopes, foot contact can induce large surface deformations and solid-fluid-like transitions, coupling terrain effects with robot dynamics, leading to underperformance or failure. This is partly due to the lack of reliable methods to represent the dynamics of flowable terrain, making it difficult to account for terrain effects in locomotion design. Here, we investigate how controlling terrain response can improve bipedal locomotion on granular slopes by studying the terradynamics of cleated feet, thin plates emanating from the foot soles. Systematic studies of a small-scale (1.4 kg) robophysical biped reveal that cleats with sparse and dense spacing lead to excessive terrain yielding and resistance, respectively, degrading performance and leading to failure. An intermediate cleat spacing distributes interaction forces to maintain substrate stresses near (or below) the yield threshold, enabling walking on granular slopes up to 30 degrees. Guided by these principles, we design a foot that actively adjusts cleat depth and accommodates both rigid and granular terrain. We also demonstrate that the principles of effective foot-terrain interaction translate to a larger (15 kg) autonomous biped. Our study presents an alternative to conventional body-centric robot control approaches, which regulate terrain-induced effects through body motion, by instead regulating terrain interactions through limb-centric approach.

Abstract

Contact-Implicit Trajectory Optimization

Explore similar work

Robust bipedal locomotion on flowable slopes via foot-driven terrain manipulation

Multi-Terrain Mastery: A Comprehensive Controller for Bipedal Locomotion

Perceptive Variable-Timing Footstep Planning for Humanoid Locomotion on Disconnected Footholds