SURE: Safe Uncertainty-Aware Robot-Environment Interaction using Trajectory Optimization

TL;DR

SURE introduces a robust trajectory optimization framework for contact-rich robotics by explicitly accounting for contact-timing uncertainty through a branching phase that pre-positions multiple potential pre-impact states and enforces a rejoining to a common final trajectory. This approach balances robustness with computational efficiency by avoiding full multi-branch post-impact trees and leveraging a common trajectory to keep decision variables manageable. Across cart-pole and ball-catching tasks, SURE improves success rates and caps impact velocities under uncertainty, and real-world egg-catching experiments validate substantial performance gains over nominal planning. The method enables trajectory scheduling when contact timing can be sensed and robust-nominal planning when sensing is unavailable, offering a practical and scalable path toward reliable loco-manipulation in uncertain environments.

Abstract

Robotic tasks involving contact interactions pose significant challenges for trajectory optimization due to discontinuous dynamics. Conventional formulations typically assume deterministic contact events, which limit robustness and adaptability in real-world settings. In this work, we propose SURE, a robust trajectory optimization framework that explicitly accounts for contact timing uncertainty. By allowing multiple trajectories to branch from possible pre-impact states and later rejoin a shared trajectory, SURE achieves both robustness and computational efficiency within a unified optimization framework. We evaluate SURE on two representative tasks with unknown impact times. In a cart-pole balancing task involving uncertain wall location, SURE achieves an average improvement of 21.6% in success rate when branch switching is enabled during control. In an egg-catching experiment using a robotic manipulator, SURE improves the success rate by 40%. These results demonstrate that SURE substantially enhances robustness compared to conventional nominal formulations.

Figures (10)

• Figure 1: Illustration of the egg-catching task. The Unitree Z1 robot arm attempts to catch a falling egg while minimizing impact. The egg serves both as the test object and as a passive sensor for detecting contact forces. In this task, timing deviations on the order of milliseconds can significantly affect performance, highlighting the importance of robustness to contact-timing uncertainties.
• Figure 2: (a) The nominal trajectory optimization problem. The contact occurs at the node indexed by $c$, separating the pre- and post-impact phases. (b) The method in zhao2024trajectory, where the contact may occur at any node within the robust phase. (c) The Tree OCP QP frison2020hpipm, which is a brute-force formulation that requires an extremely large number of nodes, leading to high computational cost. (d) SURE trajectory optimization problem. The gray nodes represent the pre-contact trajectory, the green nodes correspond to the branching phase (serving the same role as the robust phase in (b)), and the blue nodes denote the common final trajectory. Together, they form the common trajectory. The orange nodes represent the branch trajectories, each diverging from a branching node and rejoining the common final trajectory, composing the rejoining phase.
• Figure 3: Illustrations of (a) trajectory scheduling, and (b) the robust nominal trajectory. Trajectory scheduling utilizes all optimized trajectories obtained from the robust approach, whereas the robust nominal trajectory corresponds to the middle branch selected from these solutions.
• Figure 4: Illustration of the cart-pole system with wall contact. The cart-pole starts from an initially disturbed state and, due to limited control input, moves toward the wall to induce an impact that reverses the pole's velocity. After the impact, the system seeks to regain balance and return to the desired position. The wall position is uncertain within a range of $\pm d$.
• Figure 5: Robustness comparison under Initial Condition 4 for three reference trajectories. The nominal wall position is $x_{\mathrm{wall}}=-0.5\,\mathrm{m}$ and the nominal restitution coefficient is 0.8. SURE trajectory optimization is performed using 5 branches, with the half-width of the uncertainty range fixed at $d=0.05\,\mathrm{m}$. The wall position varies within $x_{\mathrm{wall}} \in [-0.7\,\mathrm{m}, -0.3\,\mathrm{m}]$, and the restitution coefficient ranges from 0.7 to 0.9. A total of 200 points are randomly sampled within this two-dimensional uncertainty space, and simulations are conducted for all three approaches to evaluate and compare their success rates.