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.
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.
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