Differentiable GPU-Parallelized
Task and Motion Planning
We provide additional explanations and videos of our experiments.
Method
Particle-Based Optimization for TAMP. We search over plan skeletons, where each skeleton $\pi$ induces a continuous Constraint Satisfaction Problem (CSP) that defines the structure of a particle (parameters) and cost functions (constraints). Given $\pi$, we initialize a batch of particles, roll them out, and compute their costs. As the rollout and cost functions are differentiable, we use a gradient-based optimizer to iteratively update the particles towards convergence.
Franka Tetris
In this domain, the robot's objective is to pack blocks with non-convex shapes somewhere in a tight goal region. This problem requires reasoning about spatial arrangements, as the shapes will only fit if they are arranged in particular configuration modes.
We optimize for placement poses, parametrized as continuous 4-DOF actions with positions $(x, y, z)$ and yaw angles $\theta$, along with their associated 7-DOF robot joint positions. We sample a fixed set of grasps and use an off-the-shelf motion planner (cuRobo) to solve for the full trajectories.
We visualize the placement poses corresponding to the best particle as optimization progress.
We theorize increasing the number of particles allows us to explore multiple basins in parallel, improving the chance of finding better global solutions. The results are over 10 seeds on a Tetris problem with 3 blocks.
We show the full execution of the TAMP plan. We use the solutions for placement poses and robot configurations from the optimization stage as input to a motion planner. Note that the grasp poses are sampled from a known set.
Real-World Experiments
We deploy our TAMP algorithm to a real robot arm. In these experiments, we optimize for the grasp poses, placement poses, and trajectory knot points. Hence, we show that we can jointly optimize all the continuous variables in a plan skeleton.
Note that due to how the arm has been mounted and limited optimization, the trajectories may look a bit unnatural. There were also some bugs in the controller when we ran these experiments, causing some drift in the reached configurations.
The goal is for the robot to stack the blue block 🟦 onto the green block 🟩.
The soft cost here is to minimize the placement pose of the mustard bottle on the table in the $x$-axis.
The goal is for the robot to pick the apple 🍎 and place it into the bowl 🥣.
2D Triangle Packing
The objective in this domain from Yang et al. 2023 is to pack all the triangles into the tray while ensuring no overlap between them. The decision variables here are the $(x, y)$ positions and the rotation $\theta$ of each triangle.
This problem is challenging due to the tight constraints, which means that simply randomly sampling positions and rotations for the triangles would have a very small probability of success. Although specialized algorithms exist for such packing problems with convex-like objects, we show that our general optimization-based approach can reliably solve them too.
We show the optimization of 5 triangles, and include a soft cost to maximize the $x$-positions of the triangles. Evidently, the triangles are pushed to the right of the tray.
We randomly initialize the triangles and their poses, and optimize over 1024 particles. We show the average time to find a valid solution. The results here are over 30 runs.
Our approach achieves better results compared to diffusion or learning-based models (see Figure 5a in Yang et al. 2023), which achieve only around 80% and 60% success on 4 and 5 triangles, respectively. This is despite being trained on 30,000 instances with 2-4 triangles. Note the comparison in success rate is not equivalent as our method uses optimization, while the diffusion and learning-based models perform a fixed number of resampling steps.
October 2024 — Licensed under a Creative Commons Attribution 4.0 International License.