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

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.

💡 Research Summary

The paper introduces SURE (Safe Uncertainty‑Aware Robot‑Environment interaction), a trajectory optimization framework that explicitly handles uncertainty in contact timing, a critical challenge for contact‑rich robotic tasks. Traditional trajectory optimization methods assume deterministic contact events, which limits robustness when perception errors, modeling mismatches, or actuation delays cause the actual impact to occur earlier or later than planned. Existing approaches either rely on frequent replanning (MPC) that cannot capture the fast dynamics of impact, or use robust phases that do not provide a concrete plan to reach the final goal, or brute‑force tree‑based formulations that become computationally intractable.

SURE’s core idea is a “branch‑rejoin” structure. The uncertain contact interval is represented by a set of pre‑impact nodes (the branching phase K). From each node, the impact is modeled by a reset map R, generating a post‑impact state. Instead of planning a full trajectory from every possible post‑impact state to the goal (as in a tree OCP), SURE forces all branches to rejoin a common final trajectory. This dramatically reduces the number of decision variables while still accounting for all possible contact timings. The mathematical formulation (Eq. 2) combines a common‑trajectory cost, branch‑specific costs, and constraints that enforce dynamics, guard conditions, reset mapping, and rejoining continuity. The uncertainty width d can be fixed or treated as an optimization variable, allowing the solver to maximize the admissible uncertainty region.

Two usage modes are described for control. If a contact sensor is available, a “trajectory scheduling” scheme switches the reference to the branch whose impact time is closest to the detected contact, enabling the system to follow the most appropriate pre‑computed trajectory in real time. If no sensor is present, a “robust nominal trajectory” is selected, typically the middle branch, which offers a balanced worst‑case performance across the uncertainty set.

The framework is evaluated on two benchmark tasks. In the first, a cart‑pole system must strike a wall to reverse the pole’s velocity and then balance. The wall’s position is uncertain within ±d. SURE’s branch‑rejoin trajectories allow the system to recover from early or late impacts, achieving a 21.6 % higher success rate than a nominal deterministic planner. In the second task, a Unitree Z1 robotic arm attempts to catch a falling egg; millisecond‑scale timing errors dramatically affect impact forces. By planning multiple branches and rejoining, SURE improves the catching success rate by 40 % compared with a conventional planner. Both experiments are solved using CasADi and IPOPT; the number of branches |K| and the uncertainty width d are shown to trade off robustness against computational load.

The paper acknowledges limitations: increasing the number of branches raises computation time linearly, and the reset map R must accurately capture contact physics (e.g., friction, compliance) to avoid performance degradation on real hardware. Future work is suggested in adaptive branch generation based on uncertainty distributions, learning‑based reset models for more complex contact phenomena, and extending the approach to other uncertain parameters such as friction coefficients or contact surface geometry.

In summary, SURE provides a principled, computationally efficient method to incorporate contact‑timing uncertainty directly into trajectory optimization. By structuring the problem as a branching phase followed by a shared rejoining phase, it achieves significant robustness gains while keeping the optimization tractable, making it a promising tool for safe, uncertainty‑aware robot‑environment interaction.