TrajectoTree: Trajectory Optimization Meets Tree Search for Planning Multi-contact Dexterous Manipulation

Claire Chen, Preston Culbertson, Marion Lepert, Mac Schwager, Jeannette Bohg

Abstract

Dexterous manipulation remains a significant challenge in robotics due to its hybrid nature, requiring simultaneous reasoning over discrete contact sequences and continuous force trajectories. We present TrajectoTree, a hybrid planning framework that decouples these challenges into a two-stage process: a high-level discrete tree search for contact sequences followed by a low-level constrained trajectory optimization. Our approach achieves a 7x speedup over state-of-the-art Contact-Implicit Trajectory Optimization (CITO) methods while producing significantly more robust plans that maintain grasp stability during execution in physics simulators.

Introduction

Achieving human-level dexterity with multi-fingered robotic hands is a long-standing goal. The primary difficulty lies in the "hybrid" complexity of the task: an agent must decide which fingers to move (discrete) and how much force to apply (continuous) while ensuring the object does not slip or fall.

Currently, Contact-Implicit Trajectory Optimization (CITO) is the standard approach, attempting to solve for motion and contact forces simultaneously. However, CITO often suffers from extreme computational costs due to non-smooth search spaces and frequently generates "mathematically optimal" but physically fragile solutions—such as tossing objects to reposition fingers—that fail under real-world control.

Methods: The TrajectoTree Pipeline

TrajectoTree addresses these issues by guiding the optimization process through a two-stage pipeline.

The TrajectoTree pipeline: from initial configuration to discrete planning, constrained optimization, and final simulation.

1. High-Level Discrete Planner

Instead of allowing the optimizer to search the entire contact space, TrajectoTree utilizes a depth-first tree search to identify a feasible contact sequence. This planner prioritizes kinematics and grasp stability, following a "finger gaiting" strategy where only one finger moves at a time while the others maintain a stable grasp (form closure).

2. Low-Level Constrained CITO

The resulting high-level plan is used to define hard constraints for a CITO solver. By fixing contact locations and timing based on the tree search, the search space is drastically narrowed. This "rescue" of the optimizer from high-dimensional physics space leads to faster convergence and more reliable solutions.

The final trajectories are executed in the PyBullet physics engine using a standard impedance controller with gravity compensation.

Evaluation and Results

The framework was evaluated on a simulated benchmark involving a planar four-fingered hand tasked with rotating a rectangular object to 60 different goal orientations in a gravity-enabled environment.

Performance and Efficiency

TrajectoTree demonstrated a significant performance advantage over general CITO baselines. The proposed method achieved a 7x speedup, with an average solve time of 14 seconds compared to 98 seconds for the baseline.

Comparison of solve time and optimization cost between TrajectoTree and CITO baselines.

Robustness and Consistency

Beyond speed, TrajectoTree produced more stable trajectories. While baseline CITO methods often resulted in the object falling (indicated by large errors in vertical position), TrajectoTree maintained a steady grasp. Furthermore, the variance in planning time was much lower for TrajectoTree, indicating high reliability across diverse goal poses.

Discussion and Future Work

TrajectoTree demonstrates that imposing discrete contact constraints—often seen as a burden—can actually simplify complex optimization problems. By prioritizing stability over "acrobatic" agility, the system generates trajectories that more closely resemble human manipulation strategies.

Future research will focus on extending this framework to 3D manipulation, which involves more complex SO(3) geometry and a larger branching factor. Additionally, the team aims to automate the selection of contact switch counts and explore more scalable search methods for hands with higher degrees of freedom.

Summary updated

BibTeX
@inproceedings{chen2021trajectotree,
  author = {Claire Chen and Preston Culbertson and Marion Lepert and Mac Schwager and Jeannette Bohg},
  title = {{TrajectoTree: Trajectory Optimization Meets Tree Search for Planning Multi-contact Dexterous Manipulation}},
  booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems},
  year = {2021},
  url = {https://openreview.net/forum?id=nwuVWhLuUFG},
}
Figure