Constraining Gaussian Process Implicit Surfaces for Robot Manipulation via Dataset Refinement

TL;DR

This work proposes an online learning and optimization method to identify and avoid unobserved obstacles online to design a Model Predictive Control (MPC) method that leverages the obstacle estimate to complete multiple manipulation tasks.

Abstract

Model-based control faces fundamental challenges in partially-observable environments due to unmodeled obstacles. We propose an online learning and optimization method to identify and avoid unobserved obstacles online. Our method, Constraint Obeying Gaussian Implicit Surfaces (COGIS), infers contact data using a combination of visual input and state tracking, informed by predictions from a nominal dynamics model. We then fit a Gaussian process implicit surface (GPIS) to these data and refine the dataset through a novel method of enforcing constraints on the estimated surface. This allows us to design a Model Predictive Control (MPC) method that leverages the obstacle estimate to complete multiple manipulation tasks. By modeling the environment instead of attempting to directly adapt the dynamics, our method succeeds at both low-dimensional peg-in-hole tasks and high-dimensional deformable object manipulation tasks. Our method succeeds in 10/10 trials vs 1/10 for a baseline on a real-world cable manipulation task under partial observability of the environment.

Figures (6)

• Figure 1: Our method learns a continuous model of the obstacle geometry as an implicit surface, voxelized here for visualization, while enforcing constraints on the model. We model contacts as pairs of points interior and exterior to the 0-level-set surface. a) A constraint violating surface where the cable state estimate penetrates the surface due to noisy estimates of interior and exterior points. b) The surface after estimated contacts have been refined.
• Figure 2: Block diagram showing the algorithm. Green blocks refer to objects generated by COGIS. The dynamics model, shown here as a MuJoCo simulation, and visual input are created offline and used to plan a trajectory along with the current GPIS. After an action is executed, we infer contacts from the transition $(\textbf{X}_t, \textbf{u}_t, \textbf{X}_{t+1})$, which we use to update the GPIS. The generated data $D$ are refined using CMAwM by selecting a subset $\bar{D}$ that ensures the GPIS satisfies provided constraints $h_{\mathrm{all}}$. We fit a surface in yellow that approximates the obstacle geometry occluded by the table while constraining the surface to avoid penetration with the cable state estimate.
• Figure 3: a) The green transition has a small discrepancy between the predicted and actual next state, resulting in no contact detection. The red transition has a higher discrepancy, resulting in contact detection. b) When the cable is pulled upwards, the red transition would result in contact detection but does not due to visual pre-processing. Points that are updated by vision are indicated by the red line surrounded by green.
• Figure 4: The peg-in-hole environments. The end-effector is grasping a peg, which the robot navigates to the hole. Environments and figure are from TAMPC.
• Figure 5: Predicted obstacles for the Peg-U task. a) A constraint violation; the goal is encompassed by the obstacle, violating the constraint. b) Optimized surface; CMAwM removes the red interior point above the goal, satisfying the constraint.