Spectral Decomposition of Inverse Dynamics for Fast Exploration in Model-Based Manipulation
Authors: Solvin Sigurdson, Benjamin Riviere, Joel Burdick
Organizations: Department of Control and Dynamical Systems, California Institute of Technology, Pasadena, CA 91125, USA · Department of Mechanical and Aerospace Engineering and the Department of Computer Science, New York University, New York City, NY 10012, USA
Planning long duration robotic manipulation sequences is challenging because of the complexity of exploring feasible trajectories through nonlinear contact dynamics and many contact modes. Moreover, this complexity grows with the problem's horizon length. We propose a search tree method that generates trajectories using the spectral decomposition of the inverse dynamics equation. This equation maps actuator displacement to object displacement, and its spectrum is efficient for exploration because its components are orthogonal and they approximate the reachable set of the object while remaining dynamically feasible. These trajectories can be combined with any search based method, such as Rapidly-Exploring Random Trees (RRT), for long-horizon planning. Our method performs similarly to recent work in model-based planning for short-horizon tasks, and differentiates itself with its ability to solve long-horizon tasks: whereas existing methods fail, ours can generate 45 second duration, 10+ contact mode plans using 15 seconds of computation, demonstrating real-time capability in highly complex domains.
Figures & tables
Fig. 1 : Our method efficiently plans manipulation trajectories through several contact modes, discovering behavior like pushing, sliding, rolling, tumbling, and pivoting. In the physics-based simulator, an object (green prism) is moved with non-prehensile manipulation (red dot) through a contact-rich environment to a goal configuration.
Fig. 2 : Top left: Variables for the problem setup. Bottom right: Simple example of the fingertip exhibiting extrinsic dexterity in order to move an object through a maze.
Fig. 3 : An illustration of the fingertip controller. The dark green arrows are the linearized motion vi projected forward Tproj seconds, the gray arrows are the nonlinear motion, and the end of the red line is the fingertip reference point.
Fig. 4 : Left: For a single configuration where the object is pinned between the fingertip and the environment, proposed motion directions vi are shows with red arrows, while the true reachable set for the center of the box is shown in pink. Right: Same as the left image except for a set of 5 different initial configurations to capture sampling different contacts between the object and manipulator. The approximate reachable set for the object’s COM is shown in gray, and efficiently captures a distinct subset of all possible motions in pink.
Fig. 5 : Pictoral representation of the MDP tree for a simple maze problem. In grey are the possible motion paths for the center of mass, while in dark green is a possible path chosen by the RRT algorithm.
Fig. 6 : Short-Horizon Extrinsic Dexterity Tasks
Method
Metric
PP
CF
BR
BF
HP
PS
Ours
Success
1.0
1.0
1.0
1.0
1.0
1.0
Time [s]
6.04
2.06
0.75
3.02
3.28
2.56
Modes
2.6
4.7
2.5
6.1
6.7
6.0
Branches
73.5
60.1
11.7
82.4
90.5
54.7
Bsl. 1 [ 17 ]
Success
0.9
0.05
0.0
0.25
−
−
Time [s]
3.1
17.9
> 30.3
10.3
−
−
TABLE I : Performance on Short-Horizon Tasks
Fig. 7 : Plots of tree expansion using Baseline 1 [ 17 ] for 2D and 3D pusher tasks. Tree branches shown in blue. 3D tasks collapse to breadth first search.
Fig. 8 : Plots of tree expansion after 60s of computation in the 3D maze environment
Method
Success
Time [s]
Modes
Branches
Length [m]
Ours
1.0
15.0
9.34
436
3.90
Abl. 1
1.0
35.1
13.8
389
4.46
Abl. 2
0.96
20.55
10.8
492
3.32
Abl. 3
0.74
20.0
11.9
591
3.04
Abl. 4
0.88
13.3
31.1
634
3.15
Bsl. 2 ∗
0.0
> 32.5
∼
∼
∼
TABLE II : Ablation (Abl.) and Baseline (Bsl.) Performance. ∗ Ablations 1–4 have 50 trials and Baseline 2 has 10 trials.
Multi-robot manipulation requires jointly reasoning about contact formations, robot motions under coupled dynamics, and collision avoidance. Systematically searching over this large space is difficult and becomes increasingly intractable as the number of robots grows, the task horizon lengthens, or the scene becomes more densely cluttered. Existing approaches therefore either learn to solve the problem end-to-end via reinforcement learning or restrict planning to a simpler surrogate problem, such as planning object motions while learning short-horizon contact primitives. However, neither paradigm scales to the problem instances we target: long-horizon multi-robot manipulation in extremely dense environments. In this paper, we propose Long-horizon Adaptive Manipulation Planning (LAMP), a framework combining a generative model for manipulation with classical planning for long-horizon reasoning. We instantiate our framework with two algorithms leveraging insights from established planning techniques, A* and lazy search: LAMP-A*, which systematically searches over the coupled object-robot space, and LAMP-Lazy, a lazy planner that enables real-time replanning through deferred evaluation. Experiments in challenging simulated environments demonstrate that our approach solves complex long-horizon tasks in highly cluttered environments that prior methods cannot handle.
Shuai Zhou, Yorai Shaoul, Jiaoyang Li
Robotics Institute, Carnegie Mellon University, Pittsburgh, PA 15213, USA.
Scaling up datasets is highly effective in improving the performance of deep learning models, including in the field of robot learning. However, data collection still proves to be a bottleneck. Approaches relying on collecting human demonstrations are labor-intensive and inherently limited: they tend to be narrow, task-specific, and fail to adequately explore the full space of feasible states. Synthetic data generation could remedy this, but current techniques mostly rely on local trajectory optimization and fail to find diverse solutions. In this work, we propose a novel method capable of finding diverse long-horizon manipulations through black-box simulation. We achieve this by combining an RRT-style search with sampling-based MPC, together with a novel sampling scheme that guides the exploration toward stable configurations. Specifically, we sample from a manifold of stable states while growing a search tree directly through simulation, without restricting the planner to purely stable motions. We demonstrate the method's ability to discover diverse manipulation strategies, including pushing, grasping, pivoting, throwing, and tool use, across different robot morphologies, without task-specific guidance.
We propose an optimization-based framework for robust contact-rich manipulation. Recent contact-implicit methods enable online hybrid planning across contact modes, allowing closed-loop manipulation for a given target state and contact location sequence of the robot and object. However, most existing approaches lack the ability to autonomously reason and generate diverse contact location sequences and manipulation trajectories, i.e., active contact location selection, which limits their applicability to relatively simple tasks. Active contact location selection is challenging due to complementarity in contact dynamics and the sparse gradients, making the design of a unified framework for contact selection and planning difficult. To address these challenges, we introduce Simultaneous Contact Selection and Planning (SCSP), a cascaded optimization framework comprising Contact Selection Optimization (CSO) and Contact Planning Optimization (CPO). CSO leverages a surrogate contact model and discrete-continuous optimization to efficiently resolve the nonsmoothness and coupling in contact selection, enabling online global searching of optimal contact locations. CPO performs prior-guided contact planning by evaluating the reference contact locations produced by CSO and generating corresponding manipulation trajectories in real time for redundant manipulators. Extensive simulations and real-world experiments demonstrate that SCSP produces diverse manipulation behaviors and robust control under inaccurate dynamics and perceptual noise. We further validate the generalization of the framework on challenging manipulation tasks. Project website: https://sites.google.com/view/scsp-robot.