💻 computer science

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

SURE is a robust trajectory optimization framework that explicitly models contact timing uncertainty by branching and rejoining trajectories, significantly improving success rates in robotic tasks with discontinuous dynamics compared to conventional deterministic approaches.

Original authors: Zhuocheng Zhang, Haizhou Zhao, Xudong Sun, Aaron M. Johnson, Majid Khadiv

Published 2026-06-23

📖 4 min read☕ Coffee break read

CC BY 4.0

Original authors: Zhuocheng Zhang, Haizhou Zhao, Xudong Sun, Aaron M. Johnson, Majid Khadiv

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are trying to catch a falling egg with a robotic hand. The problem is, you don't know exactly when the egg will hit your hand. Maybe it falls a split second faster, or maybe a tiny bit slower.

If you program the robot to move based on a "perfect guess" (assuming the egg will hit at exactly 2.00 seconds), and the egg actually hits at 2.01 seconds, the robot might be in the wrong position, drop the egg, or smash it. This is the problem with most current robot planning: they assume the world is predictable, but in reality, it's messy and uncertain.

This paper introduces a new method called SURE (Safe Uncertainty-Aware Robot-Environment Interaction) that helps robots plan for the "what ifs."

The Core Idea: The "Branching Path" Analogy

Think of planning a robot's movement like planning a road trip where you don't know exactly when you'll hit a traffic jam.

The Old Way (Nominal Planning): You plan a single route assuming the traffic jam happens at exactly 2:00 PM. If the jam happens at 2:05 PM, you are stuck in a dead end because your plan didn't account for the delay.
The "Brute Force" Way (Tree Search): You plan every single possible route for every possible traffic time. If the jam could happen at 2:00, 2:01, 2:02, etc., you calculate a unique route for each. This is incredibly safe, but it takes so much computer power that the robot might freeze before it even starts moving.
The SURE Way: This is the paper's innovation. Imagine you drive along a main road (the common path) until you reach a fork in the road where the traffic jam might happen.
- If the jam happens early, you take Branch A.
- If it happens late, you take Branch B.
- The Magic Trick: No matter which branch you take, they all merge back together onto the same main road a few miles later, leading you to your final destination.

SURE calculates this "branching and merging" plan all at once. It doesn't just plan for one scenario; it plans for a range of possibilities, but it forces all those possibilities to meet back up at a safe point. This makes the plan robust (it works even if things go slightly wrong) but keeps the computer work manageable (it doesn't have to calculate a unique path for every single second of the day).

How They Tested It

The researchers tested this idea with two real-world scenarios:

1. The Cart-Pole Swing (The Gymnast)
Imagine a robot cart with a pole on top of it. The robot has to swing the pole and slam it against a wall to stop it from falling over. The problem? The robot doesn't know exactly where the wall is; it could be a few inches closer or further away.

Result: When they used the old "perfect guess" method, the robot failed about half the time because it misjudged the wall. When they used SURE, the robot figured out a strategy that worked even if the wall was in the wrong spot. It improved the success rate by 21.6%.

2. The Egg Catch (The Delicate Task)
A robot arm tries to catch a falling egg. The egg might be released a tiny fraction of a second earlier or later than expected.

Result: The old method often smashed the egg because the robot's hand was moving too fast or at the wrong angle when the egg arrived. The SURE method taught the robot to move more smoothly and wait in a "safer" position, ready to catch the egg whether it arrived early or late. In real-world tests, this boosted the success rate by 40%.

Why This Matters

The paper shows that by allowing the robot's plan to "branch out" to handle uncertainty and then "merge back" to a shared goal, we can make robots much more reliable.

It's not just about being safe: It's about being efficient. The SURE method is smart enough to handle the uncertainty without needing a supercomputer to calculate millions of separate paths.
It changes the strategy: The robot doesn't just react to uncertainty; it changes its entire movement style to be more forgiving. For example, in the egg-catching task, the SURE robot moved differently before it even touched the egg, giving itself a bigger safety margin.

In short, SURE teaches robots to stop assuming the world will behave perfectly and instead plan a flexible route that works no matter how the timing shifts, all while keeping the computer calculations fast enough for real-time use.

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

1. Problem Statement

Robotic tasks involving intermittent contact with the environment (e.g., locomotion, manipulation) result in hybrid dynamical systems with discontinuous dynamics. Standard Trajectory Optimization (TO) for these systems typically assumes deterministic contact events (e.g., exact timing of impact). However, in real-world scenarios, environmental perception is uncertain, and modeling errors or tracking inaccuracies often lead to discrepancies between planned and actual contact times.

Existing robust control methods address this uncertainty primarily at the feedback level without adapting the nominal trajectory, limiting performance in scenarios like delayed contact. Other approaches, such as Reference Spreading or Tree Optimal Control Problems (Tree OCP), either lack long-term optimality or suffer from prohibitive computational complexity due to brute-force enumeration of all possible branching trajectories. The core challenge is to develop a TO framework that explicitly accounts for contact timing uncertainty to enhance robustness while maintaining computational efficiency.

2. Methodology

A. Nominal Formulation

The paper first establishes a baseline using a multiple-shooting TO formulation. This assumes contact occurs at a specific, deterministic node $c$ . The optimization minimizes a cost function subject to system dynamics, state/input constraints, and a guard function $g(x)$ that triggers the contact event (transition from free motion to impact dynamics).

B. The SURE Framework

SURE (Safe Uncertainty-Aware Robot-Environment Interaction) introduces a novel formulation that explicitly models contact timing uncertainty without the exponential cost of full tree-based enumeration.

Branching and Rejoining Topology:
Instead of planning separate trajectories from every possible post-impact state to the terminal state, SURE constructs a branching phase ( $K$ ) covering the range of possible pre-impact states. From each node in this phase, a trajectory branches out. Crucially, all these branches are constrained to rejoin a single, shared final trajectory at a specific node ( $K_e + 1$ ).
- Common Trajectory: The pre-contact path and the post-rejoining path are shared.
- Branching Phase: Represents the uncertainty window where contact could occur.
- Rejoining Phase: Short trajectories connecting the post-impact state of each branch back to the common final trajectory.
Optimization Formulation:
The objective function minimizes the running cost over the common trajectory, the costs over all rejoining branches, and the terminal cost.
- Uncertainty Modeling: A variable $d$ represents half the width of the uncertainty range. Constraints enforce that the guard function $g(x)$ equals $d$ at the earliest possible contact node ( $K_0$ ) and $-d$ at the latest ( $K_e$ ).
- Constraints: Each branch must satisfy dynamics and constraints from its specific post-impact state until it merges back into the common trajectory.
Control Execution Strategies:
- Trajectory Scheduling: If contact sensing is available, the controller switches to the specific branch trajectory corresponding to the detected contact time.
- Robust Nominal Trajectory: If sensing is unavailable, the system follows a single "robust nominal" trajectory. This is typically the middle branch (equidistant from the uncertainty extremes), which is optimized to perform well across the entire uncertainty range, even if it is not the optimal path for any single specific realization.

3. Key Contributions

Unified Robust TO Framework: SURE explicitly accounts for contact timing uncertainty by allowing trajectories to branch and rejoin, balancing robustness with computational efficiency.
Computational Efficiency: By forcing branches to rejoin a common trajectory, SURE avoids the "curse of dimensionality" associated with full Tree OCP formulations, significantly reducing decision variables and computation time.
Experimental Validation: The paper provides the first experimental validation of such a robust formulation in a real-world setting (egg catching), moving beyond purely theoretical or simulation-based claims.

4. Results

Case Study I: Cart-Pole System

Task: Stabilizing a cart-pole system using an impact with a wall at an uncertain location ( $\pm d$ ).
Simulation: Compared against a nominal TO and a Tree OCP approach.
- Success Rate: SURE with trajectory scheduling achieved a 66.4% success rate across four initial conditions, compared to 44.8% for the nominal approach.
- Robust Nominal: Even without scheduling (using only the middle branch), SURE improved the success rate to 55.3%, demonstrating that the optimization process itself reshapes the pre- and post-contact motion to be more robust.
- Efficiency: Compared to Tree OCP, SURE reduced total computation time by 39.78% and post-warm-start time by 76.05%, with only a 4.85% increase in the theoretical optimal cost.

Case Study II: Egg Catching (Real-World Experiment)

Task: A Unitree Z1 robotic manipulator catching a falling egg with uncertain release timing (simulated by varying the delay $t_{wait}$ ).
Experiment: Physical trials compared the nominal trajectory against the SURE robust nominal trajectory.
- Success Rate: SURE achieved an overall success rate of 85%, compared to 45% for the nominal trajectory. This represents a 40% improvement in success rate under timing uncertainty.
- Impact Analysis: The robust nominal trajectory maintained a lower maximum relative impact velocity (2.67 m/s vs. 3.93 m/s for nominal) across varying drop heights, effectively mitigating the risk of breakage.

5. Significance and Claims

The paper claims that SURE substantially enhances robustness compared to conventional nominal formulations without incurring the high computational costs of exhaustive tree-based methods.

Strategic Adaptation: The results demonstrate that SURE does not merely adjust local motion near the contact event; it reshapes the global motion strategy (e.g., altering approach angles and buffering distances) to accommodate uncertainty.
Practical Viability: The successful real-world egg-catching experiment validates that the theoretical framework translates effectively to physical systems with inherent actuation and sensing delays.
Future Directions: The authors note that the framework is currently limited to single-contact events and plan to extend it to multiple sequential contacts and floating-base systems (loco-manipulation) in future work. They also aim to learn uncertainty-conditioned policies based on the robust formulation.