6D Robotic Assembly Based on RGB-only Object Pose Estimation

6D object pose estimation has received a lot of attention in both robotics and computer vision communities in recent years. Some works conduct indirect approaches to predict the 6D pose. A popular approach is to establish 2D-3D correspondences, which are subsequently exploited for computing the 6D poses by P$n$P algorithm. For instance, BB8 [14] and YOLO6D [15] compute the 2D projections of 3D bounding box corners. To enhance the robustness, SegDriven [16] and PVNet [17] employ segmentation paired with voting for each correspondence. Meanwhile, CDPN [18] and DPOD [19] predict dense rather than sparse correspondences and achieve remarkable performance. Similarly, EPOS [20] represents the object by compact surface fragments to handle symmetries.

Another branch of works directly regresses the 6D pose. PoseCNN [21] and DeepIM [22] leverage a point matching loss and CosyPose [23] extends DeepIM [22] by introducing multi-view information and implementing global scene refinement. SingleStage [24] and GDR-Net [13] infer intermediate 2D-3D correspondences and directly regress the 6D pose via the network rather than the P$n$P algorithm, showing that the learned P$n$P is capable of producing more robust estimates than standard P$n$P. Additionally, SSD6D [25] discretizes the pose space and conducts classification rather than regression.

Noteworthy, the majority of these methods exploit annotated real data for training. However, labeling real data requires tremendous consumption of time and labor. Hence, some works completely rely on synthetic data to avoid this defect [26, 27]. Though the performance of these methods falls behind those using real annotations, the domain gap between synthetic and real images can be narrowed by physically-based rendering [28, 29] or domain randomization strategies.

With the blossom of deep learning, vision-based robotic grasping and assembly progress rapidly. Compared with robotic grasping tasks [30, 31], robotic assembly tasks require higher precision because the grasped object interacts not only with the gripper but also with the target object. Peg-in-hole insertion tasks and block stacking tasks have drawn much attention among others.

Peg-in-hole insertion tasks require robots to assemble gears, shafts, etc. Various challenges have been proposed in the last few years, e.g., the NIST Assembly Task Boards [1] and the World Robot Summit (WRS) Assembly Challenge  [2, 3]. The targets are typically placed on a plane, thus many peg-in-hole insertion methods leverage 2D object detection [4] or 3D pose estimation (i.e., the 2D object center $(x,~{}y)$ and the orientation angle $\theta$) [6]. [5] utilizes YOLOv5 to detect target objects and accomplish assembly using visual and force servoing. Their interaction within the 3D space is limited and the flexible multi-angle assembly is beyond their reach.

Meanwhile, several works deal with vision-based robot stacking tasks by reinforcement learning [9, 10], trying to teach robots to stack one block on top of the other block. Their methods leverage few prior and rely on physical simulation to perform large-scale robot training, which proves the possibility of learning a vision-based policy to stack multiple object combinations. However, only simple tasks, i.e., stack one block on top of the other, can be conducted. There is still a long way towards high-precision block stacking tasks leveraging reinforcement learning.

Recently, some works combine pose estimation and robotic assembly or stacking. [11] introduces a new robotic assembly task, which requires reasoning about local geometry that is surrounded by arbitrary context and formulates the task as the 6D pose estimation of template geometries. [12] conducts plug insertion, box packing and cup stacking tasks leveraging RGBD-based 6D object pose tracking along with within-hand visual feedback control.

For training the 2D object detection network and the 6D object pose estimation network, we generate photorealistic images leveraging physically-based rendering. Only 3D object models are required in this process. Since the blocks are standard and regular, we manually model them by SolidWorks and re-sample the surface utilizing Poisson-disk sampling to obtain dense point clouds.

Some 6D pose estimation methods [22, 18, 17] render 3D object models in an arbitrary pose and randomly choose a background image from PASCAL VOC [32] or SUN397 [33] dataset to generate synthetic data. However, the generated images are unrealistic due to the fragmented foreground and background, which triggers the domain gap between synthetic and real data.

In contrast to them, we exploit BlenderProc [29], a physically-based rendering pipeline, to generate synthetic training and validation data. Specifically, each object is assigned with a uniformly sampled initial pose and then dropped to the ground of a room with a random photorealistic material from the CC0 Textures library. The synthetic images are rendered leveraging Blender with properly set light and camera positions. Since the Pybullet physics engine records the real-time object poses, we simultaneously acquire both 2D detection and 6D pose annotations of the images.

As shown in Fig. 4 (a), abundant high-quality images with precise annotations can be generated for any given model, which is highly efficient and labor-saving. Our methodology greatly narrows the domain gap between synthetic and real images. Subsequently, the 2D detection network and 6D pose estimation network, which are solely trained on synthetic data, can be directly applied to real scenes without further training or using domain randomization techniques.

Given an RGB image and a set of objects together with their corresponding 3D models, we aim to estimate the 6D pose $P=[R|t]$ w.r.t. the camera for each object, with $R$ representing the 3D rotation and $t$ denoting the 3D translation.

Fig. 3 presents a schematic overview of the proposed methodology. We follow GDR-Net [13] to directly regress the 6D pose. Leveraging physically-based rendering, we train the 2D detection network and the 6D pose regression network solely on synthetic data. Specifically, we first detect all objects of interest using the trained detection network. For each corresponding Region of Interest (RoI), we feed it to the pose network exploiting Dynamic Zoom-In (DZI) [18] strategy, which decouples the 2D detection process and 6D pose estimation process. Then several intermediate geometric feature maps indicating the 2D-3D correspondences are predicted, and finally, a CNN module that simulates the P$n$P algorithm is exploited to directly regress the 6D object pose.

Following [13], we employ a disentangled 6D pose loss, individually supervising the rotation $R$, the scale-invariant 2D object center [18] and the distance. We parameterize rotation $R$ as the first two columns of the rotation matrix, which has been demonstrated effective [34, 13].

To sum up, with a glance of the camera mounted to the gripper, we acquire the current poses of all blocks w.r.t. the camera by our 6D pose estimation methodology. Aided by the high quality of training data and the generalization ability of the network, our system requires no prior of working scenario, which vastly extends its application range.

Based on the 6D poses we have acquired, the robot system is capable of interacting with the blocks. For implementing subsequent grasp operations, we first consider two coordinate systems: the object coordinate system $\mathnormal{O}_{obj}$, defined in the 3D object model, and the robot base coordinate system $\mathnormal{O}_{base}$. The 6D pose represents the transformation $T^{cam}_{obj}$ from $\mathnormal{O}_{obj}$ to the camera coordinate system $\mathnormal{O}_{cam}$. With the offline calibration between the camera and the robot flange and the robot kinematics parameters, the transformation from $\mathnormal{O}_{obj}$ to $\mathnormal{O}_{base}$ is obtained by

$$T^{base}_{obj}=T^{base}_{flange}~{}T^{flange}_{cam}~{}T^{cam}_{obj},$$

(1)

where $T$ denotes the homogeneous transformation matrix. For simplification, we consider the axis of $\mathnormal{O}_{obj}$ with the minimum angle to the vertical axis of $\mathnormal{O}_{base}$ as the reference axis

$$I_{ref}=\arg\max_{v}\langle e_{z},~{}R^{base}_{obj}~{}v\rangle,$$

(2)

where $v$ denotes the unit vector of the axes of $\mathnormal{O}_{obj}$ and $e_{z}$ denotes the unit vector of the vertical axis of $\mathnormal{O}_{base}$.

We conduct a grasp towards the block center along the reference axis to avoid the unreachable point of the robot arm and collisions with the other blocks. However, in the assembly process, a proper grasp position needs to be found for the gripper to avoid collisions with the other blocks. Considering the blocks are regular, we conduct grasping along the axes in $\mathnormal{O}_{obj}$, including $\pm x$, $\pm y$ and $\pm z$. For each direction, we pre-define two orthogonal grasps. For instance, the grasp along $+x$ includes two orthogonal grasps in $x$-$y$ plane and $x$-$z$ plane. Considering directly grasping the object center may be infeasible due to the collisions with the designed structure, we also pre-define two extra grasp positions for each direction, with offsets relative to the center. To sum up, 36 grasp candidates are pre-defined for each block in $\mathnormal{O}_{obj}$.

Knowing the block poses and the grasp candidates, we detect collisions between the gripper and the designed structure leveraging the Flexible Collision Library (FCL) [35] to exclude infeasible grasp candidates. Noteworthy, the 2-finger gripper can be formulated as two cuboids, which is enough for filtering out infeasible grasp candidates.

We design a target structure manually in Blender, where the relative 6D poses between each block can be obtained. Simultaneously, the assembly sequence is also allocated. As long as we assign the absolute 6D pose of the first block in $\mathnormal{O}_{base}$, all the poses of blocks can be inferred, which makes our method easy to transfer to other desired structures.

Therefore, the arbitrary initial and target block poses have been obtained respectively, which guides the robot to implement 6D transformation for each block. Similar to the grasping process, the rotation along the vertical axis of $\mathnormal{O}_{base}$ is easy to deal with while the rotation along the other two axes may be out of range due to the limited workspace of the robot arm. We would like to avoid wide angle rotation along the horizontal axes of $\mathnormal{O}_{base}$.

If the target pose shares the same reference axis with the current pose, the assembly can be conducted simply by virtue of the rotation along the vertical axis of $\mathnormal{O}_{base}$. Otherwise, we move the block to a rotation workspace and rotate it along one horizontal axis of $\mathnormal{O}_{base}$ to make the target reference axis upturned. Noteworthy, up to two rotations are sufficient for any arbitrary conditions and the number of rotations is related to the angle between the reference axes of the current pose and the target pose. Note that to account for symmetries, we compute all correct poses by multiplying the estimated pose and rotation matrix of symmetry and select the most convenient transformation strategy.

Although we achieve relatively accurate 6D pose estimation, the assembly precision is still not satisfied. To be specific, in 6D pose estimation tasks, the average error of model points is usually considered acceptable up to 10% of the diameter. The grasp and place operation shares a similar tolerance and can still be implemented stably and robustly. However, in assembly tasks, the pose estimation error can lead to collisions or gaps. Considering the block only interacts with the gripper in the grasp and place operation but needs to interact with both the gripper and other blocks in the assembly operation, the precision required by the assembly operation is much higher than that required by the grasp and place operation. To eliminate the impact of pose error on assembly, we propose 3-axis calibration to decouple 6D pose estimation and assembly process.

Assume that there is a small rotation error $\Delta R$ and translation error $\Delta t$ between ground-truth pose $(\bar{R},~{}\bar{t})$ and estimated pose $(R,~{}t)$. We first grasp and place the block on a plane. Since the block is regular, $\Delta R_{x}$, $\Delta R_{y}$ and $\Delta t_{z}$ can be eliminated due to the limitation of the plane. Then we conduct two orthogonal grasps along the x axis and the y axis of $\mathnormal{O}_{obj}$ to the estimated pose, forcing the block to shift to the estimated pose. Therefore, $\Delta R_{z}$, $\Delta t_{x}$ and $\Delta t_{y}$ are eliminated, which are tiny yet crucial to assembly process.

Noteworthy, 3-axis calibration shares the same plane prior with the preceding process, thus does not leverage supernumerary prior. On account of 3-axis calibration, the blocks can still be anywhere with arbitrary initial poses, even be stacked together. The pose error during perception, grasp and place process cannot be accumulated to the assembly process, thus the success rate is improved dramatically.

The robotic system consists of a KUKA LBR iiwa R820 robot arm and a 2-finger Robotiq 2F-140 gripper, with an Intel RealSense D435i camera mounted to the gripper. The offset between the camera and gripper is obtained in an off-line calibration procedure. We conduct robot experiments leveraging Robotic Operating System (ROS). IIWA STACK [39], a ROS metapackage for KUKA LBR iiwa robots, is employed to communicate between ROS and KUKA Sunrise software.

We design four structures for assembly (Fig. 5 (a)) and conduct 15 trials for each structure. As demonstrated in Table II, our methodology achieves a 100% 2D detection rate and the 6D poses of all blocks are predicted, respectively. A trial is considered successful if all the blocks are properly assembled without severe collision and the assembly gap is less than 1mm as well. Among all the 60 trials, 52 are successful with the target structure constructed, achieving an 86.7% success rate. To further analyze the whole construction process, we consider the perception, grasp, manipulation and assembly of a single block as a step. A trial consists of a different number of steps according to the number of blocks. We achieve a 93.5% step success rate among all the steps in four structures.

Noteworthy, the target structures are designed with tight tolerance and no gap is reserved between the blocks, thus only 1mm error is admissible during the assembly process. With a precise and robust 6D pose estimation algorithm, pose-guided 6D transformation strategy and 3-axis calibration, the assembly with tight tolerance is successfully implemented as shown in Fig. 5.

The system error is principally derived from 6D pose estimation and calibration process. When the block is severely occluded or in certain ambitious conditions, our system may perceive a rough 6D pose. In most cases, the grasp can still be implemented and then the 6D pose error is eliminated by 3-axis calibration, which contributes to one successful assembly. Conversely, the 6D pose error that exceeds the tolerance of robotic grasping leads to a grasping failure (Fig. 6 (a)).

For the assembly error less than 1mm, only slight collisions and weeny gaps occur with no damage to the whole structure, while the assembly error larger than 1mm may wreck the whole structure (Fig. 6 (b)). To summarize, our robotic assembly system is capable of assembling blocks with a tolerance of 1mm by RGB-only input.

To demonstrate the effectiveness of 3-axis calibration, we attempt to assemble structure 1, which consists of only four blocks, without exploiting the 3-axis calibration operation for the last 3 blocks. Experimental results show that 13 out of 15 trials failed with severe collision and the collapse of the whole structure (Fig. 6 (c)). Our 3-axis calibration operation outperforms direct assembly by a large margin, which indicates the indispensability of the disentanglement of 6D pose estimation and assembly process.

It is worth noting that we make some assumptions in our approach, e.g., blocks with regular shapes, flat surfaces and stable flipping poses. Further exploration is needed for more general robotic assembly tasks.