V-MAO: Generative Modeling for Multi-Arm Manipulation of Articulated Objects

We consider the problem of multi-arm manipulation of articulated objects. The object has $N$ rigid parts and the system contains $N$ robot arms where $N\geq 2$. Robot arm $i$ executes high-level grasping action $g_{i}$ and moving actions $a_{i}$ on the objects. Grasping action $g_{i}$ is represented by the contact points on object part $i$. Moving actions $a_{i}$ send torque signals to robot $i$ to move object part $i$ to desired 6D pose $\mathcal{T}_{i}$. The whole manipulation is a sequence of $g_{i}$ and $a_{i}$.

Given colored 3D point clouds from RGBD sensors, suppose $\mathbf{p}_{i}=\{\mathbf{x}_{i,j}\in\mathbb{R}^{3+C},j=1,\ldots,n_{i}\}$ is the 3D point cloud of the $i$-th rigid part of the object, where $n_{i}$ is the number of points and $C$ is the number of feature channels, e.g. color values and motion vectors. Suppose $\mathbf{p}_{S}^{(k)}=\{\mathbf{x}_{j}^{(k)}\in\mathbb{R}^{3+C}\}$ is the 3D point cloud of the non-object scene and $\mathcal{T}_{i}^{(k)}$ is the 6D pose of $i$-th rigid object part after executing grasping actions $g_{1},g_{2},\ldots,g_{k}$. Note that the complete 3D point cloud of the scene $\mathbf{p}^{(k)}=\mathbf{p}_{S}^{(k)}\bigcup(\bigcup_{i=1}\mathbf{p}_{i})=\mathcal{O}(\prod_{l=1}^{k}g_{l},\prod_{l=1}^{N}\mathcal{T}_{l}^{(k)})$ is a function of grasp actions $(g_{1},g_{2},\ldots,g_{k})$ and object part poses $\mathcal{T}_{i}^{(k)}$, where $\mathcal{O}$ is the observation of the environment and depends on robot forward kinematics. We assume there is no cycle in object joint connections so that there is no cyclic dependency among object parts. We formulate the manipulation problem as learning the following joint distribution of $(g_{1},g_{2},\ldots,g_{n})$ given initial scene point cloud $\mathbf{p}^{(0)}$:

$P_{i}$ is the contact point distribution model of $i$-th rigid part. In Equation (1), we use conditional probability to decompose $P$ and assume previous point clouds have no effect on the current grasping distribution. Our goal is to learn distribution $P_{i}$ for each object part that covers the feasible grasping as widely as possible so that sampled grasping actions $g_{i}$ and subsequent moving actions $a_{i}$ can successfully take all object parts to desired poses $\mathcal{T}_{i}$.

Our generative model is based on variational inference [5]. Different from 2D images where the positions of the pixels are fixed and only the pixel value distribution needs to be modeled, in 3D point clouds, both geometry and point feature distribution need to be modeled. Therefore, instead of directly using multi-layer perceptions (MLPs) on pixel values similar to vanilla VAE, our model needs to learn both local and global point features in hierarchical fashion [23]. Moreover, in our model, the reconstructed feature map should not only be conditioned on latent code, but hierarchical geometric features as well.

Encoder and Decoder for One Single Step. Given a colored 3D point cloud appended with successful grasping probability maps $\mathbf{p}=\{\mathbf{x}_{j}\in\mathbb{R}^{3+4}\}$ where four feature channels are RGB and probability values, our model split the feature channels to construct two point clouds appended with different features: a 3D color map $\mathbf{p}_{\text{rgb}}=\{\mathbf{x}_{j}\in\mathbb{R}^{3+3}\}$ as the input to geometric learning, and $\mathbf{p}_{\text{c}}=\{\mathbf{x}_{j}\in\mathbb{R}^{3+1}\}$ as the 3D contact probability map to reconstruct using generative model.

An encoder network $\mathcal{F}_{\text{c}}(\cdot;\theta_{\text{c}})$ is used to approximate the posterior distribution $q(z\mid\mathbf{p}_{\text{c}},\mathbf{p}_{\text{rgb}})$ of the latent code $z$. Another encoder network $\mathcal{F}_{\text{rgb}}(\cdot;\theta_{\text{rgb}})$ learns the hierarchical geometric representation and outputs features $\mathbf{h}_{1},\mathbf{h}_{2},\mathbf{h}_{3}$ at different levels:

where $\mu$ and $\sigma$ are the predicted mean and standard deviation of the multivariate Gaussian distribution respectively. The latent code $z$ is then sampled from the distribution and is used to reconstruct the contact probability map on point clouds with a decoder network $\mathcal{G}(\cdot;\theta_{\text{g}})$:

Note that both $\mathcal{F}_{\text{c}}(\cdot;\theta_{\text{c}})$ and $\mathcal{G}(\cdot;\theta_{\text{g}})$ are conditioned on $\mathbf{p}_{\text{rgb}}$. Therefore, our generative model can be viewed as a conditional variational encoder (CVAE) [24] that learns the latent distribution of the 3D contact probability map $\mathbf{p}_{\text{c}}$ conditioned on the deep features of 3D color map $\mathbf{p}_{\text{rgb}}$. The conditional variational formulation takes observation $\mathbf{p}_{\text{rgb}}$ into account and can improve the generalization.

The encoders $\mathcal{F}_{\text{rgb}}(\cdot;\theta_{\text{rgb}})$ and $\mathcal{F}_{\text{c}}(\cdot;\theta_{\text{c}})$ share the same architecture and are instantiated with set abstraction layers from PointNet++ [23]. The Decoder $\mathcal{G}(\cdot;\theta_{\text{g}})$ is instantiated with set upconv layers proposed in [25] to propagate features to existing up-sampled point locations in a learnable fashion.

Loss Function. Our generative model is trained to maximize the log-likelihood of the generated contact probability map to be successful contact labels. Due to Jensen’s inequality, the evidence lower bound objective (ELBO) in variational inference can be derived as

The total loss $\mathcal{L}=\mathcal{L}_{\text{recon}}+\mathcal{L}_{\text{latent}}$ consists of two parts. Reconstruction loss $\mathcal{L}_{\text{recon}}=-E_{z\sim q(z|\mathbf{p}_{\text{c}},\mathbf{p}_{\text{rgb}})}\log P(\hat{\mathbf{p}}_{c}|z,\mathbf{p}_{\text{rgb}})=-E_{z\sim q(z|\mathbf{p}_{\text{c}})}\log P(\hat{\mathbf{p}}_{c}|z)=H(\mathbf{p}_{c},\hat{\mathbf{p}}_{c})$ is the cross entropy between $\mathbf{p}_{c}$ and $\hat{\mathbf{p}}_{c}$. Latent loss $\mathcal{L}_{\text{latent}}=D_{KL}(\mathcal{N}(\mu,\sigma^{2})||\mathcal{N}(0,1))$ is the KL Divergence of two normal distributions. The goal of the training is to minimize the total loss $\mathcal{L}$. The parameters in the three networks, $\theta_{\text{rgb}}$, $\theta_{\text{c}}$ and $\theta_{\text{g}}$, are trained end-to-end to minimize $\mathcal{L}$.

Given the predicted contact points on object part $i$, the 6D pose of the end-effector of robot $i$ can be obtained by solving an inverse kinematics problem. Path planning algorithm such as RRT [26] is used to find an operational space trajectory from initial pose to desired grasping pose. If a path is not found, the framework will iteratively sample contact point distributions from the generative model multiple times. Then we use Operational Space Controller [27] to move the gripper along the trajectory and finally complete the grasping action.

After a successful grasp, the next goal is to apply torques on robot arms and gripper to move the object part $i$ to the desired pose. To keep the grasping always valid, there should be no relative motion between the gripper and the object part. We propose Object-centric Control for Articuated Objects (OCAO) formulation. It computes the desired change of the 6D poses of robot end effectors given the desired change of the 6D pose of one of the object rigid parts and the changes of all joint angles. Operational Space Control (OSC) [27] will then be used to execute the desired pose change of the robot end effectors. For more technical details on the mathematical formulation of OCAO, please refer to the supplementary material.

Part-level Pose Estimation. To ensure the object contact point is present in the 3D point cloud, we merge the colored object mesh point clouds and the scene point cloud as a combined point cloud. The 6D pose of each object part is estimated using DenseFusion [28], an RGBD-based method for 6D object pose estimation. DenseFusion is trained separately using the simulation synthetic data. Since adding object mesh point cloud to the scene point cloud causes unnatural point density, we use farthest point sampling (FPS) on the merged scene point cloud to ensure uniform point density.

Automatic Label Generation from Exploration. Training the variational generative model requires a large amount of data. To collect sufficient data for training, we propose to automatically generate labels of contact point combinations using random exploration and interaction with the simulation environment. Expert-defined affordance areas of grasping, for example the handle of the plier, are provided to ensure realistic grasping and narrow down the exploration range. We provide initial human-labeled demonstrations of possibly feasible contact point combinations, for example a pair of points at the opposite sides of the plier handle, to speed up exploration efficiency. We then use random exploration on the contact points within the affordance region and verify the contact point combinations through interaction in the simulation environment. Successful contact point combinations are saved in a pool used in training. For more details on the algorithm of random exploration, please refer to the supplementary material.

Task and Environment. We conduct the experiments in an environment constructed with MuJoCo [3] simulation engine. The environment consists of two robots facing a table in parallel. The articulated object is placed on the table surface with random initial positions and joint angles. The goal of the manipulation is to grasp the object parts and move them to reach the desired pose and joint angle configuration within a certain error threshold.

Objects and Robots. We evaluate our approach on six daily articulated objects from PartNet [4] in the experiments. The objects include three pliers and three laptops illustrated in Figure 3. For the robots in the experiment, we use Sawyer and Panda which are equipped with two-finger parallel-jaw Rethink and Panda grippers. Both robots and grippers are instantiated by high-fidelity models provided in robosuite [29]. For the friction between robot gripper and the object, we assume a friction coefficient of 0.65 and assume pyramidal friction cone.

Baseline. There is no related previous work on multi-arm articulated objects that we can compare our V-MAO with. So we developed a baseline approach denoted as “Top-1 Point” that predicts a deterministic per-point contact probability map on the point cloud using an encoder-decoder architecture and selects the best contact point for each gripper finger within the affordance region. To fairly compare, the baseline uses the same encoder and decoder architectures and is trained on the same interaction data as V-MAO. After contact point selection, the subsequent planning and control steps are also the same for the baseline.

We explore two types of grasping strategy, sequential grasp and parallel grasp. In sequential grasp, object parts are grasped and moved in an sequential fashion. In $i$-th step, robot gripper $i$ grasps the $i$-th object part while leaving parts $i+1$ through $N$ uncontrolled. We iterate the above step until all parts are grasped. In parallel grasp all object parts are grasped simultaneously. Then the robots move the object parts to the goal locations together.

We report the success rate of manipulation in Table 1. Our V-MAO can achieve more than 80% success rate with Sawyer robot and more than 70% success rate with Panda robot on all objects, which shows V-MAO can effectively learn the contact distribution and complete the task. Besides, on the Panda robot, sequential grasping generally achieves a much better success rate than parallel grasping. This means determining grasping for other robots later may be a better option than determining both robots’ grasping at the beginning in certain cases.

We notice that when using the Panda robot, the performance of both V-MAO and the baseline decrease significantly. The reason is that the Panda robot has a large end effector size. Therefore when manipulating small objects like pliers, in many cases the space is not enough to contain both grippers due to robot collision, while Sawyer robot’s end effector is much smaller. The deterministic prediction baseline achieves a larger success rate on laptops. A possible explanation is that there are less variation in grasping laptops compared to pliers, for example the first grasp of the laptop is almost always grasping the base horizontally. Thus a deterministic model can be more stable in simpler tasks. On the other hand, in environments with larger variability, a generative model can have advantage.

In Figure 4, we visualize the probability map on the 3D point clouds sampled from the generative model given different latent codes $z$. As expected, the maximum value probability value falls on the object points. The points that have the highest probability for the respective gripper also form valid grasps. Given different latent codes $z$, the generated contact points can cover a wide range of feasible contact point combinations. We also notice that there are some object regions that are less covered by the generative model during sampling. One possible reason is that grasping in these regions can introduce collisions in the current or the next steps and have a lower probability of being sampled.

In Figure 5, we visualize the executed sequential manipulation using the contact points sampled from the generative model. As we can see, the robot gripper can successfully reach the contact points after inverse kinematics and path planning. When both grippers are grasping the object, the sampled and generated contact points combination for both robots can also avoid collision between robots. After grasping, both robots can reliably control the object to reach the goal configurations marked by green point cloud.