Literature
Display
← All papers

Physics-Driven Data Generation for Contact-Rich Manipulation via Trajectory Optimization

Lujie Yang · H. J. Terry Suh · Tong Zhao · Bernhard Paus Græsdal · Tarik Kelestemur · Jiuguang Wang · Tao Pang · Russ Tedrake

Preprint posted · arXiv (preprint) ↗

Robotics9 min text · CondensedOriginal paper ↗
Contents

Overview

PhysicsGen uses human demonstrations for global task guidance and local trajectory optimization for physical feasibility. Generated trajectories train state-based diffusion policies; bimanual iiwa policies are evaluated directly on hardware.

Physics-driven data generation overview; see Figure 1 caption.
Fig. 1: Physics-driven data generation overview. Leveraging trajectory optimization, our framework automatically generates thousands of dynamically feasible contact-rich trajectories across a range of embodiments and physical parameters from only 24 human demonstrations. The policy trained with imitation learning from the generated dataset is more robust and performant.

At a glance

A small set of virtual-reality demonstrations is kinematically retargeted to a robot, then refined in simulation under varied object poses and physical parameters. The optimizer adapts contacts and controls instead of only replaying a geometric transformation. The resulting trajectories train imitation policies for a floating Allegro hand and bimanual iiwa/Panda arms.

The pipeline reuses the same two-finger demonstrations across the two arm embodiments. Hardware deployment on iiwa arms reaches 17/23 successes with 500 generated trajectories versus 6/23 for the 24-demonstration baseline. This is local augmentation around demonstrated behavior with state estimation from motion capture, rather than a general visual manipulation policy.

I Introduction

Contact-rich data is expensive to collect because small positioning errors change contact modes and can make teleoperation fail. Human demonstrations supply a useful sequence of contact decisions, while a local physics-based planner can adapt the exact actions to a different morphology, friction, mass, or initial pose.

The proposed combination addresses complementary weaknesses: unguided optimization struggles with global search, and retargeted human motion need not obey robot dynamics. The paper contributes a robot-free demonstration interface, optimization-based augmentation, cross-embodiment examples, and hardware policy evaluation. Reusing arbitrary legacy datasets is a proposed application, not separately demonstrated here.

II Related Works

The paper relates its data-generation approach to teleoperation, synthetic augmentation, contact planning, demonstration-guided reinforcement learning, and cross-embodiment transfer.

II-A Data Collection for Imitation Learning

Behavior cloning depends on demonstration quality and coverage. Existing interfaces include 3D mice, puppeteering, virtual/augmented reality, and bare-hand capture. Systems such as AR2-D2, DART, ARCap, and ARMADA trade off equipment, feedback, and transfer difficulty. The distinctive contribution here is expanding a few demonstrations through physics-based planning rather than only providing a new capture interface.

II-B Data Augmentation

MimicGen and related systems split demonstrations into object-centric subtasks, transform them, and replay them to create data. SkillMimicGen combines such skills with connecting motion plans; RoboCasa also expands scenes and assets. PhysicsGen adds local dynamic refinement for tasks where whole-body contacts and frequent contact switches make end-effector-only geometric transformations fragile.

II-C Trajectory Optimization for Contact-Rich Tasks

Contact-implicit optimization models contact decisions through complementarity constraints but often depends on a good initialization. Sampling-based planners avoid some derivative difficulties and have succeeded in dexterous control, yet unconstrained behavioral diversity can make imitation data harder to learn.

The proposed recipe uses comparatively consistent human demonstrations to choose a broad strategy, then optimizes nearby trajectories. The implementation uses the cross-entropy method rather than requiring a particular contact-implicit solver. It provides empirical local planning improvements, without a global-optimality guarantee.

II-D Demonstration-Guided Reinforcement Learning

Demonstrations can guide reinforcement learning through replay buffers, policy pretraining, or rewards. Those methods optimize a policy over exploratory rollouts. This work instead first refines individual trajectories and then trains a policy by imitation. The claimed computational advantages over reinforcement learning are not established by a matched wall-clock benchmark in this paper.

II-E Cross-Embodiment Generalization

Cross-embodiment work transfers latent plans, uses portable collection tools, or trains on heterogeneous robot datasets. Here explicit kinematic correspondences and a known simulator adapt the same demonstration to a target robot. The arm experiments show reuse between iiwa and Panda; the Allegro task uses a separate four-finger demonstration interface.

III Data Collection

Apple Vision Pro tracks human hand poses, which are streamed to Drake. Drake simulates hand–object contact and returns the object pose for visualization through Vuer. No physical robot is required during capture, although the headset, simulator, and robot models remain necessary. The authors report roughly seven minutes to collect 24 long-horizon demonstrations for each considered system.

For the 22-DOF floating Allegro hand, demonstrations use four fingers of the right hand. For bimanual arms, two index fingers manipulate a small cube while wrist motion is constrained to mimic fixed bases. Retargeting scales these contacts to a larger box and two seven-joint arms. The later augmentation method can also take demonstrations from other interfaces.

Refer to caption
Fig. 2: VR-based human-hand demonstration framework.
Refer to caption
Fig. 3: Human hand demo in VR and kinematic retargeting for different embodiments. The blue spheres illustrate the demo hand landmarks scaled to the specific system.

IV Automated Data Generation

Data generation first solves a geometric matching problem and then a dynamic refinement problem. Randomized parameters produce nearby tasks, with each generated trajectory retaining the demonstrated strategy as guidance.

IV-A Kinematic Motion Retargeting

At every timestep, retargeting minimizes weighted distances between manually chosen corresponding landmarks. Constraints impose nonpenetration and robot joint limits. Robot and demonstration state dimensions may differ because their landmarks map into the same workspace.

An SQP-style solver locally linearizes collision constraints and quadratically approximates the matching objective. The previous timestep’s solution initializes the next one for continuity and speed. This produces a kinematically plausible sequence; it does not by itself ensure that contact forces or object motion are physically feasible.

Equations 1a–1c
qtretarget⋆=arg​minqtretarget\displaystyle{q_{t}^{\text{retarget}}}^{\star}=\argmin_{q_{t}^{\text{retarget}}}\quad ∑i=0Nwi​‖ψi​(qtretarget)−ψ~i​(xtdemo)‖2\displaystyle\sum_{i=0}^{N}w_{i}\|\psi_{i}(q_{t}^{\text{retarget}})-\tilde{\psi}_{i}(x^{\text{demo}}_{t})\|^{2} (1a)
s.t.   ϕj​(qtretarget)≥0,∀j\displaystyle\phi_{j}(q_{t}^{\text{retarget}})\geq 0,\;\forall j (1b)
qmin≤qtretarget≤qmax,\displaystyle q_{\text{min}}\leq q_{t}^{\text{retarget}}\leq q_{\text{max}}, (1c)

IV-B Demonstration-Guided Trajectory Optimization

The dynamic program tracks the retargeted robot and object states while penalizing control effort, subject to simulated dynamics, nonpenetration, state bounds, and control bounds. Object tracking receives greater weight than robot tracking. Human guidance supplies contact timing and rough locations, while optimization adjusts actions for the target embodiment and sampled physical parameters.

The retained algorithm samples perturbations and refines the trajectory for each sample. It assumes the demonstration remains useful near the perturbed task. Simulator consistency is conditional on the chosen physical model; finite optimization does not guarantee success, and unsuccessful samples are visible in the later generation statistics.

Table I preserves the complete augmentation ranges: position and yaw perturbations, cube size, mass, friction, and task horizons. Only horizontal translation and yaw are randomized initially so the object remains supported by the table. Hand and arm parameter ranges differ.

Equations 2a–2e
xt⋆,ut⋆=arg​minxt,ut\displaystyle x_{t}^{\star},u_{t}^{\star}=\argmin_{x_{t},u_{t}}\quad ‖xT−xTretarget‖QT2+\displaystyle\|x_{T}-{x_{T}^{\text{retarget}}}\|_{Q_{T}}^{2}+
∑t=0T−1\displaystyle\sum_{t=0}^{T-1} (‖xt−xtretarget‖Qt2+‖ut‖Rt2)\displaystyle(\|x_{t}-{x_{t}^{\text{retarget}}}\|_{Q_{t}}^{2}+\|u_{t}\|_{R_{t}}^{2}) (2a)
s.t.   xt+1=f⁡(xt,ut)\displaystyle x_{t+1}=f(x_{t},u_{t}) (2b)
ϕj​(xt)≥0,∀j\displaystyle\phi_{j}(x_{t})\geq 0,\;\forall j (2c)
xmin≤xt≤xmax\displaystyle x_{\text{min}}\leq x_{t}\leq x_{\text{max}} (2d)
umin≤ut≤umax.\displaystyle u_{\text{min}}\leq u_{t}\leq u_{\text{max}}. (2e)
Algorithm 1 Automated Data Generation
1 Input: Probability distribution ρ\rho, augmentation number NN, demo trajectory xdemo0:Tx^{\text{demo}}_{0:T};
2 Output: NN dynamically consistent trajectories on target embodiments {(x0:T⋆,u0:T−1⋆)}\{(x_{0:T}^{\star},u_{0:T-1}^{\star})\};
3 q0:Tretarget⋆←{q_{0:T}^{\text{retarget}}}^{\star}\leftarrow Solve (1) for xdemo0:Tx^{\text{demo}}_{0:T};
4 for n=1,…,Nn=1,\ldots,N do
    5 Sample θ0:T∼ρ\theta_{0:T}\sim\rho;
    6 (x0:T⋆,u0:T−1⋆)←(x_{0:T}^{\star},u_{0:T-1}^{\star})\leftarrow Solve (2) with x0:Tretarget,θ0:Tx_{0:T}^{\text{retarget}},\theta_{0:T} and xt+1=f⁡(xt,ut,θt)x_{t+1}=f(x_{t},u_{t},\theta_{t});
Parameter Floating Allegro Hand Bimanual Robot Arms
Init. obj. trans. pert. (cm) [±1.5\pm 1.5, ±1.5\pm 1.5, 0] [±5\pm 5, ±5\pm 5, 0]
Init. obj. rot. pert. (rad) [0, 0, ±0.3\pm 0.3] [0, 0, ±0.3\pm 0.3]
Object side length (cm) [5.8, 6.2] [28, 32]
Object mass (kg) [0.1, 0.3] [0.25, 0.75]
Friction coefficients [0.7, 1.3] [0.2, 0.4]
Task horizon (s) 25 50 / 260 (Panda / iiwa)
TABLE I: Ranges of different physical parameters θ\theta. The initial object pose is only perturbed in yaw, x, and y to ensure the object sits stably on the table.
Refer to caption
Fig. 4: Trajectory optimization is crucial for generating dynamically feasible trajectories. (Top) Before trajectory optimization, the kinematically retargeted demos easily lose contact and drive the object out of reach with different physical parameters or slight deviations in object states. (Bottom) Trajectory optimization encourages robots to establish contact with and maintain good manipulability of the object. The tricolor axis indicates the object orientation.

V Trajectory Optimization Experiments

The goal is to bring the object to a target pose. Success requires position error below 3 cm for the hand or 10 cm for the arms, and orientation error below 0.2 rad. The optimizer is CEM; parameter perturbations follow Table I. These goal-tolerance criteria are separate from whether a rollout obeys the simulator’s dynamics.

Naively replaying the 24 retargeted demonstrations succeeds 4, 5, and 6 times for Allegro, iiwa, and Panda. Under the listed pose/size perturbations it succeeds only 1–4 times per 24 trials. Optimized data generation succeeds 2164/3000, 2252/3000, and 2462/3000 respectively—about 72%, 75%, and 82%. The denominators and perturbation protocols differ, so these are not paired 24-trial comparisons.

A separate same-embodiment control collects24 demonstrations directly with the simulated Allegro hand. MimicGen-style replay under physical variation succeeds 51%, versus 86% after trajectory optimization. This reduces the embodiment-gap confound, but compares the paper’s replay adaptation rather than establishing a benchmark result for every MimicGen application; the text does not give the trial count for these percentages.

Qualitative examples show firmer grasps, a second arm supporting the box, and actions that keep it within reach. The same two-finger demonstrations generate trajectories for both arm types. iiwa uses conservative velocity limits for hardware, whereas simulated Panda motions use softer velocity regularization. Augmentation broadens local state coverage while staying near the source strategy.

Perturbation Allegro Hand iiwa Arms Panda Arms
Original demo 4 / 24 5 / 24 6 / 24
Object size 2 / 24 1 / 24 4 / 24
Initial object translation 1 / 24 3 / 24 2 / 24
Initial object orientation 2 / 24 3 / 24 3 / 24
Trajectory optimization 2164 / 3000 2252 / 3000 2462 / 3000
TABLE II: Success rates of replaying kinematically retargeted trajectories of the 24 original human demos, and trajectory optimization under random perturbations in physical parameters and object initial conditions.
Refer to caption
Fig. 5: Distribution and snapshots of trajectories generated from a single demonstration. (a) The original demonstration (orange) is locally perturbed and augmented to about 100 dynamically feasible contact-rich trajectories (blue) for each system. The density map represents the object pose distribution of the generated trajectories in the specific 2-dimensional slices. (b) Snapshots of 30 dynamically feasible trajectories under random physical parameters and object initial poses for bimanual iiwa arms are visualized.
Refer to caption
Fig. 6: Policy rollouts for different embodiments. The object manipulation task requires the robots to frequently make and break contact with the object. It also requires precise control of the robot since small deviations in positions can result in missing contact interactions and lead to task failure.
Refer to caption
Fig. 7: Failure cases of baselines. (a) The baseline policy trained on the original 24 demonstrations for the floating Allegro hand frequently misses contact or gets stuck on the cube. (b-c) The baseline policies for the bimanual robot arms often exhibit jittery motion, resulting in loss of contact, the box being kicked out of reach, or the robot arms running into and getting stuck on the box surface.
Refer to caption
Fig. 8: Success rates of policy evaluation in simulation and hardware.
Refer to caption
Fig. 9: Policy rollouts on hardware. The fixed-base bimanual iiwa arms perform a sequence of coordinated rolling, pitching, and yawing actions to reorient the box to the goal pose.

VI Behavior Cloning Experiments

The generated data is evaluated through downstream behavior cloning, separating the optimizer’s trajectory success from the learned policy’s closed-loop success.

VI-A Policy Evaluation in Simulation

State-based diffusion policies are trained on the 24 original demonstrations or on 500/1000 generated trajectories. Each embodiment has 48 evaluation rollouts, using the same pose tolerances as the trajectory-optimization experiment. The figure retains both data-size settings; no multi-seed training confidence intervals are reported.

VI-A 1 Floating Allegro Hand

Human hand demonstrations average about five seconds and are slowed by 2.5× for robot velocity limits. The hand policy has a 25-second task horizon to permit recovery. Its 22-dimensional configuration action includes the floating base and finger joints.

The baseline succeeds 10/48 times, about 21%; the best augmented-data policy succeeds 39/48, about 81%. The plotted 500-trajectory setting reaches about 73%. Augmented policies more often re-establish missed contacts, while the baseline can jitter, repeatedly miss the cube, or become stuck against it.

Refer to caption
Fig. 10: Policy failure and recovery on hardware. The baseline policy frequently (a) gets stuck on the box surface when small deviations from the demonstration trajectories occur, and (b) struggles to recover from out-of-distribution states, where the object is never intentionally lifted for accomplishing the task in the generated dataset. Policies trained on augmented datasets (c) sometimes fail due to unmodeled collision geometry, but (d) can recover from undesired sliding by employing firmer grasps found by trajectory optimization.

VI-A 2 Bimanual Robot Arms

The iiwa baseline succeeds 27/48 times, about 56%, and the Panda baseline 14/48, about 29%. The authors suggest iiwa’s tighter velocity limits produce longer, more quasi-static demonstrations with denser state–action coverage; this is an explanation rather than an isolated causal ablation.

The best augmented policies reach 44/48 for iiwa, about 92%, and 42/48 for Panda, 87.5%. In the plot, iiwa decreases from about 92% with 500 trajectories to 88% with 1000, while Panda increases from about 81% to 88%. Augmentation improves the baselines but does not yield a monotone data-scaling curve for every embodiment. Policies can retain clockwise and counterclockwise variants from demonstrations.

VI-B Policy Evaluation on Hardware

The hardware task flips a 30 cm cubic box with fixed-base iiwa arms. OptiTrack supplies object pose, so this zero-shot transfer does not require visual state estimation by the learned policy. No real-world policy fine-tuning is described.

Over 23 trials per policy, the 24-demonstration baseline succeeds 6 times; 500 generated trajectories yield 17 successes, about 74%, and 1000 yield 16, about 70%. The difference between the augmented settings is one trial and provides no evidence of reliable superiority of either data size.

Baseline failures include hitting the box surface and failing to recover from sliding or unfamiliar lifted poses. Augmented policies can hold the opposite side more firmly to recover from sliding, but unmodeled iiwa collision geometry still induces unwanted yaw during pitching. The complete hardware failure/recovery sequence is retained.

VII Limitations and Future Work

Human-hand guidance may miss capabilities unique to a robot, such as specialized joint motion. Local augmentation also leaves distant failure states poorly covered, so catastrophic errors can remain unrecoverable. Better planning and embodiment-aware source datasets are proposed extensions.

The demonstrated policies use robot configuration and object pose. Visuomotor training with synthetic rendering is future work. Model fidelity, manually specified correspondences, and known dynamics constrain transfer beyond the evaluated systems.

VIII Conclusion

Demonstrations provide a global contact strategy that local optimization can refine across nearby physical conditions and robot embodiments. The results support generating useful imitation data from a small capture effort and transferring a state-based bimanual policy to hardware. Broad generalization, arbitrary legacy-data reuse, and visual policy transfer remain open.

IX Acknowledgement

The authors acknowledge discussions with Huaijiang Zhu, Haonan Chen, and Jiayuan Mao, and support from the Robotics and AI Institute and Amazon.

X APPENDIX

The supplement gives the trajectory optimizer’s local horizon, sampling and cost settings, together with diffusion-policy input/output sizes and training settings.

X-A CEM Implementation Details

CEM optimizes six action knots, linearly interpolated into commands for Drake’s 200 Hz contact simulation. The local plan duration is 1.25 seconds for Allegro and iiwa, and 2 seconds for Panda; these short planning horizons differ from the full 25/50/260-second task horizons.

Each update uses 50 samples, five elites, and initial action standard deviation 0.05 in every input coordinate. State weights are 10 for object tracking and 0.01 for robot tracking; terminal state cost is ten times the stage state cost. Input weights are 0.1 for Allegro and 10 for the arms. The exact table is retained; the appendix does not state a CEM iteration budget or provide full end-to-end data-generation runtime.

Parameter TT Plan Duration qoq_{o} qrq_{r} rur_{u}
Floating Allegro Hand 6 1.25 s 10 0.01 0.1
Bimanual iiwa Arms 6 1.25 s 10 0.01 10
Bimanual Panda Arms 6 2.0 s 10 0.01 10
TABLE III: Parameters for CEM. TT: planning horizon. qoq_{o}: scalar weight for tracking object trajectories. qrq_{r}: scalar weight for tracking robot trajectories. rur_{u}: scalar weight for control input.

X-B Policy Implementation Details

A UNet diffusion policy observes robot configuration plus object pose, with rotations represented as matrices, and predicts configuration commands. All embodiments use an observation horizon of 10 and action horizon of 40.

Allegro has 34 observation and 22 action dimensions, operates at 50 Hz, and trains for 1000 epochs. Both arm systems have 26 observation and 14 action dimensions and train for 800 epochs; iiwa operates at 20 Hz and Panda at 50 Hz. These are state-based inputs, not camera images. The full source parameter table remains alongside the summary.

Parameter ToT_{o} TaT_{a} Freq Epochs Obs. Dim. Act. Dim.
Floating Allegro Hand 10 40 50 1000 34 22
Bimanual iiwa Arms 10 40 20 800 26 14
Bimanual Panda Arms 10 40 50 800 26 14
TABLE IV: Parameters for diffusion policies. ToT_{o}: observation horizon. TaT_{a}: action horizon. Freq: environment frequency (Hz, both observations and actions).
Concept demonstration

Open image ↗