This article contains details on sim2real in robotic manipulation for following tasks:

• Perception for manipulation (DOPE / SD-MaskRCNN).
• Grasping (Dex-Net 3.0 / 6DOF GraspNet).
• End-to-end policies. (Contact rich manipulation tasks & In hand manipulation of rubik’s cube)
• Guided domain randomization techniques (ADR / Sim-Opt).

The reality gap:

An increasingly impressive skills have been mastered by DeepRL algorithms over the years in simulation (DQN / AlphaGo / OpenAI Five). Both Deep learning and RL algorithms require super huge amounts of data. Moreover, RL algorithms there is risk to the environment or to the robot during the exploration phase. Simulation offers the promise of huge amounts of data (can be run in parallel and much faster than real time with minimal cost) and doesn’t break your robot during exploration. But these policies trained entirely in simulation fails to generalize on real robot. This gap between impressive performance in simulation and poor performance is known as the reality gap.

Some of the ways to bridge the reality gap are:

Illustration of Sim2Real Approaches. PC: Lil’Log [1]

• System Identification: Identify exact physical / geometrical / visual parameters of environment relevant to task and model it in simulation.
• Domain Adaptation: Transfer learning techniques for transferring / fine-tuning the policies trained in simulation in reality.
• Domain Randomization: Randomize the simulations to cover reality as one of the variations.

We’ll mainly be focussing on domain randomization techniques and their extension used in some of the recent and successful sim2real transfers in robotic manipulation.

Domain Randomization

Formally domain randomization is defined as:

P_{mu} is the randomized transition distribution. τ is the trajectory of samples as per policy π in the environment P_{mu}.

So effectively, domain randomization is trying to find a common policy π parameters that work across a wide range of randomized simulations P_{mu}. So the hope is that the policy that works across wide range of randomizations also works in the real world, assuming that the real world is just another randomization covered by randomization.

Based on how these simulation randomization are chosen we have 2 types:

• Domain randomization: Fixed randomization distributions over a range often chosen by hand. We will see how this has been used in perception & grasping tasks for data efficiency.
• Guided domain randomization: Either simulation or real world experiments can be used to change the randomization distribution. We will see how this has been used in training end2end policies for contact rich and dexterous tasks. Some of the guided domain randomizations do appear like domain adaptation.

Domain randomization:

Some of the early examples of using domain randomizations was used for object localization on primitive shapes[2] and table top pushing[3]. We will look at examples of more advanced tasks such as segmentation and pose estimation with emphasis on what randomizations were chosen and how good are the transfer performance.

Domain Randomization in Perception:

SD Mask R-CNN: SD (Synthetic Data) Mask R-CNN trains category agnostic instance segmentation entirely based on synthetic dataset with performance superior to that fine-tuned from COCO-dataset.

Data Generation Procedure for SD-Mask-RCNN. WISDOM (Wear House Instance Segmentation Dataset for Object Manipulation).

Simulator: pybullet

Randomizations: Since this network uses depth images as inputs, the randomizations needed are quite minimal ( depth realistic images are easy to generate compared to photo realistic).

• Sample a number n ∈ p(λ = 5) of objects and drop it in the bin using dynamic simulation. This will sample different objects and different object poses.
• Sample camera intrinsics K and camera extrinsic (R, t) ∈ SE(3) within a neighborhood of real camera intrinsics and extrinsic setup.
• Render both the depth image D and foreground object masks M.

The Mask-RCNN trained on instance segmentation entirely on synthetic data (SD-Mask R-CNN) is compared against a couple of baseline segmentation methods and Mask R-CNN trained on COCO dataset & fine-tined (FT Mask R-CNN) on WISDOM-real-train. The test set WISDOM-real-test used here is the real world dataset collected using a high-res and low-res depth cameras and hand labelled segmentation masks.

Performance of Mask R-CNN. For both AP (Average Precision) and AR (Average Recall) higher is better.

From the ablation study, both metrics go up as number of synthetic data samples are increased indicating more data could help the improve the performance. However, increasing the number of unique objects has mixed results (may be due limited number of objects in WISDOM-real-test).

Some qualitative comparison of segmentation results from SD Mask R-CNN

DOPE (Deep Object Pose Estimation): DOPE solves the problem of pose estimation of YCB objects entirely using synthetic dataset that contain domain randomized and photorealistic RGB images.

Simulator: UE4 with NDDS Plugin.

Domain Randomizations:

• Number of / types / 3D poses / textures on distractor objects of primitive 3D shapes.
• Numbers / textures / 3D poses of objects of interest from YCB objects set.
• Uniform / textured or images from COCO as background images.
• Directional lights with random orientation and intensity.

Photorealistic:

• Falling YCB objects in photo realistic scenes from standard UE4 virtual environments. These scenes are captured with different camera poses.

Notice that camera intrinsics randomization wasn’t necessary here since the method regresses heat-maps of 3D box and vector fields to the centroid. It uses these predicted 2D information / camera intrinsics (explicitly) / object sizes to predict the 3D pose.

ADD (Average distance of 3D points on bounding boxes) pass rate vs distance threshold plots below measures successful pose detection within that threshold (higher is better). Notice how both DR and photorealistic images were necessary to get comparable performance to method trained on real world data (PoseCNN).

Some qualitative comparisons of DOPE with PoseCNN (real data) is shown below. Notice how DOPE produces tighter boxes and more robust to lighting conditions.

Domain Randomization in Grasping:

Let’s look at some examples of domain randomizations applied to robotic grasping (both suction based and parallel jaw grasps) with emphasis on what aspects are randomized and their transfer success to real robot grasping.

Dex-Net 3.0:

Suction GQ(Grasp Quality)-CNN takes in a depth image patch centered at suction point and outputs a quality measure. The process of generating the quality measure labels is illustrated below:

Here are examples of few more labels generated with grasp robustness annotated 3D models:

Simulator: Custom quasi-static physics model that simulates seal formation and ability to resist gravity and random external wrenches.

Randomizations: The graphical model shows the randomization parameters used in Dex-Net 3.0

PC: Dex-Net 3.0

Here are the randomizations explicitly listed:

• Sample a 3D object O uniformly from training set.
• Sample a resting pose T_s and sample planar disturbance from U([-0.1, 0.1], [-1.0, 1.0], [0, 2π)) and apply the planar disturbance (x, y, θ) to T_s to obtain object pose T_o
• μ coefficient of friction is sampled from N_+(0.5, 0.1)
• Camera pose T_c is sampled in spherical coordinates (r, θ, ϕ) ∈ U([0.5, 0.7], (0, 2π, 0.01π, 0.1π)) where the camera optical axis intersections the table.
• Suction grasps are uniformly sampled on object 3D mesh surface.

For each such sampled grasp, the wrench resistance metric is computed and the point cloud for the 3D object mesh is rendered using sampled camera pose and known camera intrinsic.

Zero shot transfer of policy (CEM) that optimizes the samples according to suction GQ-CNN is shown in video below:

6DOF GraspNet:

The GraspNet framework has 2 components both of which take the point cloud corresponding to target object:

• VAE (Variational Auto-Encoder) predicts 6-DOF grasp samples that has high coverage on the target object.
• Grasp evaluator that takes 6-DOF grasp sample in addition to point cloud produced quality scores. Which is later used for refining the grasp sampled via VAE.

The gradient on the grasp evaluator can be used to further refine the sampled grasps.

Training both networks require positive grasp labels, which are generated entirely in simulation.

Simulator: NVIDIA FleX simulator.

Synthetic grasp data generation:

• An object is sampled from a subset of ShapeNet.
• An approach based sampling scheme is used for generating grasp samples. Samples that are not in collision and non-zero object volume are selected for simulation.
• Object mesh and gripper in the sampled pose are loaded in simulation. Surface friction and object density are kept constant (No randomizations ! really ?). The gripper is closed and a predefined shaking motion is executed. Grasps that keep the object between the grippers are marked as positive grasps.
• Hard negative grasps are generated in neighborhood of positive grasps that are either in collision with gripper or zero object volume between grippers.

The visualization of the grasp data generation:

Note that 6-DOF GraspNet doesn’t actually use YCB objects for training. This is just for illustrating the data generation process. PC: Billion ways to grasp

Some of the positive grasp samples on bowls and boxes are shown below:

The performance of 6-DOF GraspNet on previously unseen YCB objects:

Guided domain randomization:

Previously, we saw several examples of randomized simulations that lead to successful transfer to real robotic tasks. These randomizations were chosen carefully around the nominal real world values and often tuned for real world transfer. This either becomes tedious when there are large number of parameters to choose for and very wide randomizations often leads to infeasible / sub-optimal solutions in simulations. We will look at two strategies for automating this:

• Automatic domain randomization in the context of solving Rubik’s code.
• Sim-Opt in the context of contact rich manipulation tasks which uses real world rollouts of policy.

Automatic Domain Randomization (ADR):

Let’s take a brief look at the overall framework used for Rubik’s cube solving before delving into ADR algorithm. Here is a nice overview of the entire framework:

Overview of framework for solving the Rubik’s cube. Giiker is a “smart” Rubik’s cube that has sensing of face angles upto 5⁰ resolution.

Although, the vision part of network is also trained entirely in simulation with ADR , let’s focus on hard controller policy part that manipulates the Rubik’s cube. Note that optimal sequence of rotations of Rubik’s cube faces are solved by Kociemba’s algorithm

The task of solving the Rubik’s cube now reduces to the problem of successfully executing face rotations and flip actions to make sure the face to be rotated is on top.

The shadow robotic hand is used for performing the flip and rotations on the Rubik’s cube. Here are the details of inputs and outputs of the policy network and reward functions.

Inputs: Observed fingertip positions, observed cube pose, goal cube pose, noisy relative cube orientations, goal face angles, noisy relative cube face angles.

Outputs: shadow hand has 20 joints that can be actuated, and the policy outputs a discretized actions space of 11 bins per joint.

Reward function: Combination of:

• Distance between present cube state to goal state.
• Additional reward for achieving the goal.
• Penalty for dropping the cube.

Also, episodes are terminated based on 50 consecutive successes / dropping the cube or time out while trying to achieve the goal.

Simulator: MuJoCo

Also, a lot of effort has been put into simulating the details of Rubik’s cube dynamics and Shadow robot hand.

Visualization of Rubik’s cube task in MuJoCo simulator.

ADR Algorithm:

Compared to naive domain randomization:

• Curriculum learning: ADR gradually increases the task difficult leading easier policy converge.
• Automatic: Removes the need for manual tuning of parameters, that could be non-intuitive for large parameter set.

Randomizations:

• Simulator physics parameters such as friction between cube, robot hand, cube size, parameters of the hand model etc.
• Custom physics parameters such as action latency, time step variance.
• Observation noise to cube poses, finger positions at episode level as well as each step level.

Overview of ADR

All simulation parameters are sampled from uniform distribution over a range (ϕ_L, ϕ_H). Thus is distribution of simulator parameters for d parameters is given by:

And entropy is used for measuring the complexity of training distribution, which for product of uniform distribution is:

Task performance (i.e number of success in a given episode) thresholds (t_L, t_H) is used to adjust the parameters ϕ. ADR starts with a single simulation parameter value. At each iteration, one of the boundary (ϕ_L or ϕ_H) value of one of the randomization parameter ϕ_i is chosen and the performance is evaluated and added to a buffer (D_L or D_H). After the buffer is of adequate size, depending on whether the overall performance is above t_H or below t_L, ϕ_i range is increased or ϕ_i range is decreased respectively.

Detailed algorithm for ADR

Sim2Sim: The benefit of curricular learning was studied in the context of Sim2sim transfer of bringing the cube to goal orientation. The test set is previously hand tuned domain randomization scheme which was never presented to ADR. As can be seen, as the entropy of domain randomization goes up, as does the performance on the test simulation environment.

ADR is compared against several fixed randomization schemes that were reached via curriculum training, as can be seen ADR reaches higher performance quickly and asymptotically similar.

Sim2Real: The table below shows performance of ADR trained policy in Sim and in Real for different amounts of training. Notice how the entropy of P_ϕ keeps growing as the training progresses.

Here is a successful execution of solving the Rubik’s cube from a random shuffle:

Meta-learning perspective: Because the LSTM policy doesn’t have enough capacity to remember all the variations of dynamics, it learns to adapt the policy to particular instantiations of dynamics during execution (i.e online system identification).

This is studied by perturbing the memory of LSTM / changing the dynamics on the fly or restraining a random joint. As can be seen each of the perturbations, the amount of time needed to complete the sub-goal suddenly goes up as the perturbation is introduced and after several executions the policy calibrates itself to new dynamics and the performance returns to it’s corresponding baseline.

Plots showing the online system identification effects.

Sim-Opt:

Sim-opt framework is trying to find parameters of simulation distribution that makes discrepancy between observed trajectory in simulation vs in real world by executing the trained policy.

It showcases the approach with two real world robotic tasks on two separate robotic hands:

• Drawer opening with Franka Emika Panda.
• Swing peg in hole task with ABB YuMi.

Tasks solved by Sim-Opt

Here is the overview of SimOpt framework:

Overview of SimOpt

Just to recap domain randomization tries to find θ (the policy parameters) such that the same policy generalizes across several randomizations ξ ∈ P_ϕ of simulator dynamics.

R(τ) is reward of the trajectory τ generated by running the policy π(θ)

SimOpt tries to minimize the following objective w.r.t simulator parameters ϕ

D is the discrepancy measure between simulated trajectory and real trajectory when running the policy π(θ). In this paper this is weighted average of L1 and L2 distances.

To reduce the amount of real world robot execution ϕ is only updated after a policy has fully converged in simulation. The iterative updates to ϕ is done as follows:

D_KL constraint is used to ensure the stability of optimization.

Here is the full algorithm for SimOpt:

The number of iterations of sim-opt iteration is just N=3 iterations.

Here are some details of simulator randomizations. Let’s look at swing peg in hole task:

Simulator: NVIDIA FleX

Simulation Randomizations:

• Swing peg in hole tasks:

The adaptation of above simulation parameter covariance matrix and corresponding states at the end policy after fully trained.

The 1st is the initialization of covariance matrix.

Although simulation parameters are quite exhaustive the policy inputs are quite minimal. 7 DoF joint positions and 3D position of the peg are inputs to the policy. The reward function is combination of distance from peg from hole, the angle alignment with hole and task success.

The fact that SimOpt needs to run the real robot execution in the training loop seems like it’s asking for a lot. However, notice that no reward function / no full state observations are needed in the real world execution step. All that is needed is to just run the learnt policy on the real robot. This seems like on the fly system identification such that policy trained on P_ϕ(ξ) generalizes on real robot.

The video below shows execution of policy trained via SimOpt

Conclusion:

We have seen several examples of successful transfers of sim2real for perception, grasping and feedback control policies. In all the examples, a lot of care has been taken to make the simulation as realistic as possible and choosing the parameters to randomize over. We also saw examples of guided domain randomizations, that simplify the task of manual tuning during sim2real transfer and avoids the policy convergence issues due to extra wide policy specifications.

Finally, will leave you with a comic (or a cautionary tale ?)

PC: https://twitter.com/dileeplearning

References

• Domain Randomization for Sim2Real Transfer
• Domain Randomization for Transferring Deep Neural Networks from Simulation to the Real World
• Sim-to-Real Transfer of Robotic Control with Dynamics Randomization
• SD-MaskRCNN: Segmenting Unknown 3D Objects from Real Depth Images using Mask R-CNN Trained on Synthetic Data
• DOPE: Deep Object Pose Estimation for Semantic Robotic Grasping of Household Objects
• Dex-Net 3.0: Computing Robust Robot Vacuum Suction Grasp Targets in Point Clouds using a New Analytic Model and Deep Learning
• 6-DOF GraspNet: Variational Grasp Generation for Object Manipulation
• [Dexterous] Learning dexterous in-hand manipulation.
• [ADR] Solving Rubik’s Cube with a Robot Hand
• Sim-Opt: Closing the Sim-to-Real Loop: Adapting Simulation Randomization with Real World Experience

This article contains descriptions of:

• How to efficiently generate labels for 3D key-points and instance segmentation ?
• How to learn 3D key-points and instance segmentation ?
• How to use 3D key-points in pick and place / contact rich manipulation tasks ?

Bonus: Also contains details on how to generate labels and learn semantic instance segmentation.

Why learn 3D key points ?

Consider the problem of hanging any mug by handle on a rack. How can we go about solving this task using robotic manipulator ?

Task of hanging any mug on a rack. Illustration of specifying this task using semantic 3D key points. Notice how yellow key point is at center of handle for different cup variations. PC: k-PAM[5]

There are a few options:

• Demonstration and end-end learning: Collect several examples of hanging a mug on a rack, with different mugs / different back grounds / different initial conditions etc. and use Imitation Learning or Inverse RL to train the policy. but what’s a good input representation for this policy ? Although, there are some impressive demos of using raw sensor data (RGB / RGBD) as input to the policy, you could most likely benefit by using some intermediate state representation in terms of sample efficiency.
• State estimation and planning: You could choose to estimate the state of the cup and use an optimization based planner to get the cup to target state. One caveat is that this is NOT a feedback policy and we’ll see how k-PAM 2.0 extends this idea to a feedback policy based on 3d key-points.

In both cases, we need (or benefit from) object state, and we will focus on literature that use 3D key-points as this intermediate representation.

Specifying the state based on 3D target object poses is common practice, however this fails to generalize to all cups with different sizes and holder shapes as illustrated below:

Task specification is more robust for category of objects like mugs when using semantic 3D key points. PC: k-PAM[5]

Also, we will see with k-PAM 2.0 how these key-points are used for feedback control in contact rich manipulation such as peg in hole insertion / usb insertion / wiping the white board etc.

Using 3D key-points as intermediate state comes at the cost of a key-point detector. Lets first discuss how to efficiently (in terms of human time)generate these 3D key-point labels and how to train a 3D key point detector.

How to get 3D key point labels ?

(and instance segmentation labels)

LabelFusion exploits 3D dense reconstruction of the scene to generate segmentation masks and object poses. A known object mesh is aligned to 3D constructed scene first by using human labelled manual registration and then refined further by ICP (Iterative Closest Point) algorithm. Since the camera poses of scene is also estimated by 3D dense reconstruction algorithm, label fusion uses these camera poses to label the object masks and object pose for each frame in the scene. The process is illustrated below:

Label fusion for segmentation masks. PC: LabelFusion [1]

The same idea of using the 3D reconstruction to label 3D key-points on an object mesh and transforming these labels to each camera pose can be used to generate the 3D key-point labels for each depth images illustrated below. Also notice that this doesn’t necessarily need pre-built 3D meshes.

3D Key-point annotation on 3D reconstruction. PC: k-PAM [5]

The advantage of labelling on 3D reconstruction and projecting to each frame makes for efficient use of annotators time. As illustrated below for objects segmentation this is almost 10k time more efficient than per frame labelling. LabelFusion collects segmentation scenes in different backgrounds and multi-object scenes and the diagram below shows the total time of label generation per scene.

The authors were able to generate 1 million+ segmentation labels in a few days (without crowdsourcing) and 100k+ 3D key-point labels with few hours of annotation time.

Labelling using LabelFusion strategy is almost 4 orders of magnitude efficient. PC: LabelFusion

How to learn 3D key points ?

(and panoptic segmentation)

First, let’s understand how panoptic segmentation works and later we’ll see how that is extended for 3D key-points detection.

Panoptic segmentation combines two types of segmentations:

• Semantic segmentation: To tell different objects types apart, eg: mugs vs plates vs background.
• Instance segmentation: To tell different instances of object apart, eg: different plates in a pile of plates.

Illustration of panoptic segmentation architecture:

PC: Panoptic FPN [4]

Feature Pyramid Network (FPN): The FPN is used to extract rich multi-scale features which is then used for segmentation tasks.

Semantic Segmentation: For semantic segmentation, the combined multi-scale feature map from FPN is upsampled (c) to input image size and for each original pixel a class is predicted. Per pixel cross entropy loss w.r.t ground truth segmentation is used for training.

Instance Segmentation: The shared FPN features are used to propose ROI (Regions of Interest → Bounding boxes that correspond to foreground object irrespective of the class eg: all soccer kids and the ball as shown below.).

Overview of Mask-RCNN for instance segmentation. [2]

For each predicted ROI bounding box, the corresponding FPN feature is extracted using RoIAlign operator (Bilinear sampling of FPN features per ROI), these RoI features are used to perform:

• Classification: Each RoI must correspond to one of the foreground classes or background Eg: soccer player vs ball vs background.
• Bounding box refinement: A tighter bounding box around each object for more accurate detection.
• Instance mask prediction: A separate FCN (Fully Convolutional Network), predicts object masks for each object type independently (using sigmoid) and the mask corresponding to the foreground class is used as the instance mask.

Learning 2D key points using Mask-RCNN

Adding Instance Mask to Faster R-CNN with FPN. PC: Mask-RCNN [3]

Key-point prediction: For each RoI feature, another mask branch is added and the output channels correspond to each of K key-points. The visible 2D key-point is encoded as heat map across the m² region. Soft-max loss over each m² region is used for training the key-point detector.

The example below shows 2D instance segmentation and human joint key-points predicted by mask-RCNN.

Instance segmentation and key-points detected by the same Mask-RCNN PC: Mask-RCNN [3]

Learning 3D key points

For 3D key-point detection, in addition to 2D key-points described above we also need to predict depth of each key-point and need to accurately predict the occluded key-points as well.

3D Key-Point Regression. PC: Integral human pose [4]

Integral human pose uses expectation (integration) over the predicted heat map instead of taking the maximal index after soft-max to make the key-point detection loss differentiable.

For each ROI, a 2D spatial key-point location as heat-map g_i(u, v) and depth d_i(u, v) are predicted, where u, v are in image coordinates. Notice, that we will regress all key-points irrespective of the occlusions.

Since most of robotic manipulation systems include RGBD images, the depth regression part is much easier compared to RGB only. 3D Key-points in the image fame is computed by taking expectation over the spatial heat-map. These key-point are transformed to world frame using known camera intrinsic and camera extrinsic.

Estimating the image coordinates [u, v] for each 3D key-point. PC: k-PAM

Estimating the depth of each 3D key-point. PC: k-PAM

Example instance mask prediction and 3D key-points predicted by k-PAM 1.0 for mugs.

Instance segmentation and 3D key points from Integral Human Pose style key-point detector. k-PAM [5]

How to use 3D key points for manipulation ?

We’ll see a few ways of utilizing 3D key-points based representations in manipulation pipelines:

• To specify pick and place tasks on a category of objects.
• To perform feedback control for contact rich manipulation tasks on a category of objects.

The common theme in the following work is that preselected 3D key-points are used as object representation and they can be detected on a large number of objects with significant variations in shape / color using the methods described above.

k-PAM 1.0

k-PAM uses the semantic key-points for pick and place application where the target pose of the object matters such as mug placed upright on a tabletop for ANY mug.

PC: k-PAM [5]

3D semantic key-points are used in two ways in k-PAM pipeline:

Specifying the grasp points

Since there are many regions on the object that are graspable, key points can be used to specify which regions to try and grasp. There are a large number of ways ( millions ) of ways to grasp an object and this is multi-modal (i.e many regions could result in similar grasp stability), trying to narrow the regions to grasp for a category of objects could actually simplify the problem for grasp planner. But in addition to grasp point, the local dense geometry & collisions needs to be taken into account for grasp planning.

Specifying the targets

First lets see how to specify the pick and place task with key-points using an example of keeping the mug upright:

Illustration of how to specify placing the cup upright task. k-PAM [5]

• Upright constraint was specified as below:

If we grasp the mug in the observed position and the grasp is tight, keeping the mug upright can be specified as transforming key-points p_i from their observed position p_i to their target position T_{action} * p_i. So the entire pick and place can be specified using a rigid transform T_{action} on the 3D semantic key-points on a category of objects.

In general, costs and constraints can be specified as T_{action} being decision variables of following optimization problem.

General k-PAM optimization. k-PAM [5]

Here are some example costs and constraints:

• L2 distance cost on key-points.

• Half-plane constraint on key-points (eg: mug / show should be above the table).

n_{plane} and b_{plane} specify the half-plane.

• Point to plane distance cost.

n_{plane} and b_{plane} specify the plane.

• T_{action} should be within the robot reachability and avoid collisions. This is common constraint across all tasks performed in k-PAM 1.0

Let’s use these costs and constraints to understand how following tasks were specified using key-points on shoes and mugs:

PC: k-PAM [5]

Shoes on rack: The following costs and constraints were used for pick and place the shoe on rack:

• L2 distance cost on shoe key-points p1, p2, p3, p4 to their target position on the rack.
• Point to plane cost on p2, p3 and p4 to the resting plane on the rack since the sole needs to be resting on the rack.
• Half-plane constraint on all key-points for non-penetration.

In addition, the shoe is grasped around the key-point p5.

Mugs on shelf: The following costs and constraints were used for pick and place of mug on shelf.

• L2 constraint (instead of cost) on all key points to target position.
• Upright constraint on the key-points p_{top_center} and p_{bottom_center}.

Mugs on rack: The following costs and constraints were used for hanging the mug by handle on a rack. To avoid the collision between mug and rack, T_{approach} was offset from T_{action} along the rack axis.

• L2 constraint p_{handle_center} to target position on rack handle.
• L2 cost on p_{top_center} & p_{bottom_center} to their nominal position.

The table below shows the success rate of various tasks specified using the above key-point based representation:

Time lapse video of keeping different mugs on rack. PC: k-PAM [5]

Feedback control for contact rich tasks

k-PAM 2.0

k-PAM 2.0 extends the k-PAM 1.0 to perform contact rich manipulation tasks illustrated below by adding a feedback controller on the detected key-points.

Overview of the tasks solved by k-PAM 2.0 [7]

In addition, it requires the same feedback controller to work not only when there is significant intra category shape variation, but also when the grasps are different. This requirement is satisfied by adding orientation to key-points. The main motivation for using the key-point poses as opposed to global pose is that 3D key-point poses are local co-ordinate representation of the object geometry that’s relevant to the tasks and ignores other geometrics variations in the category such as size and shape of USB sticks on top.

Need for oriented key-points in k-PAM 2.0

k-PAM 1.0 was used to place the peg in the initial condition before the closed loop policy was executed for peg insertion.

Overview of k-PAM 2.0

k-PAM 2.0 assumes a manipulator with force / torque sensing at the end-effector. Which is then used to compute the force and torque at the key-point which is used as input to the feedback agent. Since the 3D oriented key-point detector already provides the transform from gripper to the key-point and object is assumed to be static w.r.t gripper, forward kinematics is used to track the key-point in real-time.

Input to the feedback control policy of k-PAM 2.0

The agent outputs the desired linear and angular velocity as shown below, which is then used generate the joint velocity commands using the 3D oriented key-point Jacobian :

Joint space velocity commands from key-point commands.

Output of the feedback control policy of k-PAM 2.0

The feedback control policy based on 3D oriented key-points significantly out performs the open-loop policy as shown in Table I and pose based policy as shown in Table II

Results of real world robot experiments compared to Open-Loop baseline and 6 DOF representation.

Conclusion:

We have very good perception systems that can detect 3D key-points on objects with significant intra-category shape and visual variations and methods like k-PAM / k-PAM 2.0 can offload these inter-category complexity of the task to a robust perception system and specify the task / feedback control policy on top to accomplish some fairly non-trivial real world manipulation tasks.

References

[1] LabelFusion: A Pipeline for Generating Ground Truth Labels for Real RGBD Data of Cluttered Scenes.

[2] Faster R-CNN

[3] Mask-RCNN

[4] Panoptic Feature Pyramid Networks

[4] Integral Human Pose Regression

[5] kPAM: KeyPoint Affordances for Category-Level Robotic Manipulation

[6] kPAM-SC: Generalizable Manipulation Planning using KeyPoint Affordance and Shape Completion

[7] kPAM 2.0: Feedback Control for Category-Level Robotic Manipulation

[8] Key-points into the future: Self-supervised correspondence in Model based RL

PC: Nature-Magazine-Robot-Hands

This article contains heuristics for following:

• Parallel jaw grasps.
• Suction grasps.
• Linear push policies for improving parallel jaw grasps.
• Toppling policies for improving suction grasps.

Grasping is one of the fundamental subtask of a robotic manipulation pipeline. Both learning based and physics / geometry based grasping methods can benefit from grasp sampling heuristics in this article. Even if you are using a large arm farm to teach your robots the skills of grasping, you can save your robots quite a lot of time with these heuristics. This article summarizes the most common grasp sampling heuristics used in literature.

Some of the common ways to use these heuristics are:

• Generating labels for learning based grasp planners (offline): 6-DOF GraspNet [4] uses these samplers for evaluation with physics based simulation. Grasps that retain the object between the gripper are considered successful after a predefined shaking motion. DexNet [2][3] evaluates these grasps based on analytic quasi-static grasp wrench space (GWS) analysis. Both methods score these sampled grasps based on how good they are in resisting disturbances. These scores are used as labels for training the grasp planners.
• During grasp synthesis (inference): DexNet [2][3] uses these sampled grasps as seeds for Cross Entropy Method (CEM), and optimizes grasps based on predicted grasp quality from GQ-CNN (Grasp Quality Convolutional Network). Traditional geometric methods, prune these candidate grasps if they are kinematically infeasible or if they result in collision between gripper and other objects or environment. The best of these samples are picked for execution.

We will summarize the details of heuristics for each type of grippers used for manipulation.

Parallel jaw grasps

Parallel jaw grasps jam the object between the grippers (Most often the grippers have rubber on them to increase the size of friction cones and thus the robustness of the grasp). Typically, the success of parallel jaw grasp depends on local geometry around the grasp point like if the grasp fits inside the gripper, friction btw gripper and object surface, mass of the object.

Force Closure: If the contact points on the object are such that forces applied on those points don’t result in slippage and can resist gravity then force closure ( object doesn’t move with respect to the gripper ) is achieved, the grasp is considered successful.

Parametrization: Parallel Jaw Grasps are typically parametrized by 6-DOF pose of the gripper with initial configuration of open gripper.

Illustration shows the friction cones and forces applied by fingers at contact points on a cuboid. This illustration doesn’t account for gravity. Picture Credit: https://arxiv.org/abs/1905.00134v2

A Billion ways to grasp [1] summarizes several heuristics for parallel jaw grippers and evaluates their precision and coverage w.r.t a uniform sampler.

Assumption: Access to the 3D triangle mesh or 3D point cloud of the object so that surface normals can be computed.

Here are the two most effective heuristics that are purely based on geometry:

Approach based samplers:

These methods are characterized by approach vector of the gripper (red-dashed line) which typically aligns with normal to the palm (purple axis).

Approach based sampler. Picture Credit: Billion ways to Grasp [1]

Pseudo code for approach based sampler:

Notations:

• G → Gripper frame.
• \(purple, red, green\) → \(z, y, x\) of the gripper frame G.
• \(\vec{p}\) → Randomly chosen point on object surface.
• \(\vec{n}\) → Surface normal at point \(\vec{p}\).
• \(d\) → distance from gripper origin \(\vec{g}\) to point \(\vec{p}\)
• \(\vec{gp}\) → Gripper approach direction.
• α → angle between \(\vec{n}\) and \(\vec{gp}\)
• β → angle between z axis of gripper frame G and gripper approach \(\vec{gp}\)

For generating each sample:

• Sample normal vector \(\vec{p}\) from the surface of the object.
• α → uniform_sample(0, π/2)
• β → 0
• d → uniform_sample(0, L)
• γ → uniform_sample(0, 2π)
• Choose the gripper sample pose \(\hat{G}\) such that sampled α, β, γ and d satisfied.
• If sample \(\hat{G}\) results in collision with the object or object volume between the fingers is zero, discard the sample.

Antipodal based samplers:

These methods sample directly on the space of possible contact points and try to exploit the grasps that create force closure.

Antipodal based sampler. Picture Credit: Billion ways to Grasp [1]

Pseudo code for antipodal grasp sampler:

Notations:

• G → Gripper frame.
• \({purple, red, green}\) → \({z, y, x}\) of the gripper frame G.
• \(\vec{p}\) → Randomly chosen point on object surface.
• \(\vec{n}\) → Surface normal at point \(\vec{p}\).
• \(s_{min}\) → distance from gripper origin \(\vec{g}\) to closest on antipodal grasp ray (Stand-off distance)
• \(\alpha\) → Angle between the normal \(\vec{n}\) and antipodal grasp ray.
• \(\gamma\) → rotation around antipodal grasp ray.

For generating each sample

• sample a point \(\vec{p}\) on the object surface
• \(\alpha\) → uniform_sample(0, \(\pi/6\))
• \(s_{min}\) → 0
• \(\gamma\) → uniform_sample(0, \(2\pi\))
• Antipodal point \(p^{\prime}\) is choosen such that farthest intersection point along the antipodal grasp ray.
• Choose grasp sample \(\hat{G}\) with the center of line segment \(\vec{p}\) \(\vec{p}^\prime\) + \(d\) and rotatated \(\gamma\) w.r.t antipodal grasp ray. If sample \(\hat{G}\) results in collision with the object or object volume between the fingers is zero, discard the sample.

Comparing the two parallel-jaw heuristics

Billion ways to grasp [1] evaluates grasps based on two metrics:

• Robust coverage: Percent of robust grasps (still successful in a small ϵ-neighborhood) sampled w.r.t oracle uniform sampler. This is very similar to recall.
• Precision: Percent of the successful grasps among the sampled.

Robust coverage vs number of grasp samples. We only look at Uniform, Approach(π/2, 0) and Antipodal(π/6), which are best in each category. (Higher is better)

Precision of each category. Approach(π/2, 0) and Antipodal(π/6), which are best in each category. Higher is better during inference.

As seen by the conclusion of Billion ways to grasp[1] from the tables, if you have a limited sampling budget antipodal sampling scheme provides both highest coverage and precision. However, asymptotically misses several ground truth grasps. These correspond to small scale features on objects and along the edges of objects.

Visual illustration of what these sampled successful grasps and robust successful grasps look like. Each point is the grasp center and notice how robust grasps are clustered around object parts that fit nicely inside the gripper.

Picture Credit: Billion ways to Grasp [1]

Suction grippers

Suction grippers form vacuum seal on the surface of the object and if that vacuum force is sufficient to resist the gravity and external wrenches, the grasp is robust. Typically suction grasp success depends on surface porousness, local geometry, mass and payload capacity of the suction gripper. These grippers are most popular for pick and place of objects in warehouse order fulfillment. DexNet 4.0 [6] which is one of the best published bin-picking system that uses composite policy between suction and parallel jaw grasps, chooses suction grasps for about 82% of attempts.

Parametrization: Suction grasps are typically parameterized by point p on the object surface and approach vector v as illustrated below.

Illustration of seal formation on non-planar surface from DexNet 3.0 [3]

Planarity Centroid Heuristic:

Since successful suction grasps prefer planar non-porous surfaces, these heuristics try to find sufficiently planar surfaces on the object that are closer to COM (Center of Mass). Approach vectors are chosen along the surface normal because large motion tangential to surface might result in vacuum seal breakage.

Pseudo code for planarity centroid heuristic:

Notations:

• \(\vec{p}\) → Suction point on the surface of the object.
• \(\vec{v}\) → Approach vector for suction grasp.
• \(COM\) → Center of Mass
• \(PC_{full}\) → Full point cloud of the scene.
• \(PC_{exclude}\) → Exclude point cloud. \(PC_{0} = \emptyset\) for the \(1^{st}\) sample.

For generating each sample:

• Fit a plane to point cloud data \(PC_{full}\) using RANSAC excluding \(PC_{exclude}\).
• Sample a planar patch of vaccum cup size on the plane closest to COM.
• \(\vec{p_i}\) → Center of the planar patch.
• \(\vec{v_i}\) → -ve of the normal at that point.
• Add inliers of the sampled patch to \(PC_{exclude}\)
• Choose the suction grasp \(G_i={p_i, v_i}\) that is closest to COM

Some examples of successful suction grasps on 3D meshes are visualized below.

Illustrates suction grasps on diverse objects from DexNet 3.0 [3]dataset. Each point is a suction grasp sample with red → failed grasp and green → successful grasp.

DexNet 3.0 [3] evaluates suction grasps in physical robot trials based on two metrics:

• Average Precision: Area under the precision / recall curve. How good is the heuristic in scoring high quality grasps ?
• Success Rate: Fraction of the grasps that were successful.

Object categories used for physical robot experiments in DexNet 3.0 [3]

How well each heuristic performed on different objects in robot physical experiments. Picture credit: DexNet 3.0 [3]. For both metrics higher is better.

As can be seen from the table above, Planarity Centroid Heuristic does quite well compared to even learnt method DexNet 3.0 [3] on basic and typical objects.

Some of the failure cases of suction grasps are categorized as below:

Failure cases from DexNet 3.0 [3]. Imperceptible objects have small holes in them or have high curvature that prevents the vacuum seal and Impossible objects are porous.

Adaptive sampler:

These methods use heuristics that exploit the geometry to generate seed samples (described above) and further optimize the grasp according to grasp quality metric. Most often these are blackbox optimization technique such as cross entropy method (CEM) that doesn’t exploit object geometry. Although CEM is an optimization algorithm used in many areas, I would still consider it a heuristic since it doesn’t exploit the object geometries while sampling.

Additional assumption: Access to grasp quality function such as DexNet 2.0 / DexNet 3.0 Grasp Quality Network (GQ-CNN) or ability to evaluate quality of grasps in realtime based on GWS.

Cross Entropy Method (CEM)

Notations:

• m → Number of iterations
• n → Number of intital grasp samples.(These are 6-DOF gripper pose \(\hat{G}\) for parallel jaw and \({\vec{p}, \vec{v}}\) for suction grasp).
• \(U\) → Set of grasp samples.
• \(\xi\) → Set of elite samples.
• \(\gamma\) → Elite percentage, subset of the initial samples from \(U\). Typically < 50%
• \(k\) → Number of mixtures used in gaussian mixture model (GMM) \(M\).
• \(Q_{\theta}\) → Grasp Quality Function.

Algorithm:

\(U\) → Uniform sample of grasps \(\hat{G}_i\). For i = 1, … m:

• \(\xi\) → top \(\gamma\) percentile of \(U\) ranked by \(Q_{\theta}\)
• \(M\) → Fit GMM to \(\xi\) with \(k\) mixtures.
• \(U\) → n iid samples from \(M\).

Return the best grasp according to \(\arg\max_{u \in U} Q_{\theta}\)

If you were familiar with CEM, you may have noticed the use of GMM instead of Gaussians and this is because distribution of grasps on most objects are multi-modal.

Some examples of applying CEM method to DexNet 2.0 (parallel jaw grasps )and DexNet 3.0 (suction grasps) grasp quality functions to generate most robust grasps.

CEM method used by DexNet 2.0 [2] Marked in Black is the grasp output by CEM, which is very close to global maximum according to the robustness predictions \(𝑄_𝜃\)

CEM method used by DexNet 3.0 [3] Also, in this case CEM method finds the best suction grasp predicted by 𝑄𝜃

Improving chances of grasping

Sometimes neither suction grasp not parallel jaw grasp is able to pick up any object in the heap. This is mostly due to inability to perceive robust grasps (occlusion) or inability to execute the perceived grasp ( collision or kinematic infeasibility ). In those cases non-prehensile ( fancy word for non-graspable ) actions are executed to either singulate the object to expose enough clearance for parallel jaw grasps or topple the object to expose a planar surface for suction grasps.

CAUTION: The following policies have not been tested on a real robot, so the results and conclusions don’t necessarily transfer.

Parametrization: Push vector (p, q) where p = {x, y, z} starting point and q = {x’, y’, z’} is the end point.

Linear Pushing

Linear pushing policies typically help with separating the object heap so that parallel jaw grasps are accessible.

Illustration of linear pushing before (left) and after (right) in simulation (above) and real robot (below). PC: [5]

Additional assumptions: Semantic instance segmentation of the objects on the bin so that each objects position on the bin is observed. Free space segmentation of the bin is also used in the linear pushing policies for choosing the push direction.

Free Space Policy:

Aims to separate the two closest objects in the heap by pushing them towards the free space.

Pseudo-code:

Notations:

• \(\hat{c_i}\) & \(\hat{c_j}\) → Center of mass estimates of two closest objects in the heap.
• \(p_i\) & \(p_j\) → Maximal free space points that are closest to \(\hat{c_i}\) & \(\hat{c_j}\) respectively.

Algorithm:

• Find 2 closest objects with COM \(\hat{c_i}\) & \(\hat{c_j}\)
• Draw lines \(\overline{c_ip_i}\) & \(\overline{c_jp_j}\) to the corresponding maximal free space points.
• For each object \(i\) find collision free (between gripper & other objects / bin) push segment that goes through COM \(c_i\) and closest to \(\overline{c_ip_i}\).
• Choose the push segment \(\overline{cp}\) with the shortest length.

Boundary Shear Policy:

Aims to separate two closest objects in the heap by pushing one of the objects along the boundary between the objects.

Pseudo-code:

Notations:

• \(\hat{c_i}\) & \(\hat{c_j}\) → Center of mass estimates of two closest objects in the heap.

Algorithm:

• Find 2 closest objects with COM \(\hat{c_i}\) & \(\hat{c_j}\)
• Construct the line \(\overline{c_ic_j}\) projected to the support surface and it’s perpendicular \(\overline{c_ic_j}_{\bot}\)
• Generate 4 possible push vectors parallel to \(\overline{c_ic_j}_{\bot}\) and passing through \(\hat{c_i}\) & \(\hat{c_j}\) in each direction.
• Choose the push direction closest to free space and collision freel.

Facilitating Grasping [5] evaluates above policies and few others in simulation in clearing the object heaps that don’t have accessible grasps and measures the confidence gain of both grasp types. As can be seen the linear pushing policies make the parallel jaw grasps more accessible than suction grasps.

Confidence gain of both parallel jaw and suction grasping policy on according Facilitating Grasping [5]

Example of before / after of linear pushing policies described above in simulated object heaps. PC: Facilitating Grasping [5].

Singulated Object Toppling

Facilitating grasping [5] also explores policies for toppling a singulated known 3D object so that quality of suction grasp after toppling can be improved.

Topping to facilitate the top-down suction grasp. PC: Facilitating grasping [5]

Assumptions: Known 3D object with known transition distribution of stable resting poses \(P(x_{t+1} / x_t, u_t)\) and access to suction grasp quality function \( V_s(x_t) \).

Max Height Policy:

Highest possible point on the object that has surface normal within 15 degree of the supporting plane normal. This policy only gets executed if \(V_s(x_{t+1}) > V_s(x_t)\).

Greedy Policy:

Pick the action that makes the expected suction grasp more accessible.

Facilitating grasping [5] evaluates these policies in simulation and compares against a policy that runs complete value iteration based on \(P(x_{t+1} / x_t, u_t)\) and \(V_s(x_t)\).

The greedy toppling policy does fairly well compared to best performing value iteration with much less runtime PC: [5]

Conclusion:

This post explored different subtasks used for grasping and several effective heuristics for them. Please explore the references for more details on learning based / more effective policies. These heuristics are meant to provide intuition on each of the grasping subtasks and how they measure up to some of the more advanced methods.

References:

[1] A Billion Ways to Grasp: An Evaluation of Grasp Sampling Schemes on a Dense, Physics-based Grasp Data Set

[2] Dex-Net 2.0: Deep Learning to Plan Robust Grasps with Synthetic Point Clouds and Analytic Grasp Metrics

[3] Dex-Net 3.0: Computing Robust Robot Vacuum Suction Grasp Targets in Point Clouds using a New Analytic Model and Deep Learning

[4] 6-DOF GraspNet: Variational Grasp Generation for Object Manipulation

[5] Facilitating Robotic Grasping using Pushing and Toppling

[6] Dex-Net 4.0: Learning ambidextrous robot grasping policies