Abstract
Model-based planning in robotics often fails when encountering unobserved obstacles or complex deformable objects. This paper introduces COGIS (Constraint Obeying Gaussian Implicit Surfaces), a framework that enables robots to build and refine models of unobserved obstacles online. By combining contact inference with a novel dataset refinement step that enforces task-specific constraints, COGIS allows robots to navigate complex environments effectively. Experimental results in simulated and real-world cable manipulation tasks demonstrate that COGIS significantly outperforms baseline dynamics-adaptation methods, achieving high success rates even in the presence of sensor noise and occlusions.
Introduction
A central challenge in robot manipulation is handling "surprises"—obstacles that are occluded or missing from initial maps. This is particularly difficult when manipulating deformable objects like cables, where high-dimensional, non-linear dynamics make it hard to distinguish between modeling errors and physical collisions. Traditional methods often attempt to adapt the dynamics model, but this is data-intensive and computationally expensive for complex objects.
COGIS shifts the focus from dynamics to geometry. It posits that it is often more robust to assume the physics model is largely correct while the environment map is incomplete. By representing obstacles as implicit surfaces that must satisfy logical constraints (e.g., "the goal must be reachable"), the robot can effectively filter out sensor noise and "hallucinated" obstacles that would otherwise impede planning.
Methodology
The COGIS framework operates in a continuous loop during task execution, integrating several key components:
- Contact Inference and Modeling: The system compares the robot's actual state to predictions from a nominal dynamics model. Discrepancies are used to infer contact points, which are then used to fit a Gaussian Process Implicit Surface (GPIS). This represents the environment as a continuous field where the zero-level set defines the obstacle boundary.
- Visual Pre-processing: Depth data is utilized to "clean" labels, ensuring that points visible in free space are not incorrectly classified as obstacles.
- Dataset Refinement: This novel step addresses spurious obstacles created by noisy data. Using CMA-ES with Margin (a derivative-free optimizer), COGIS identifies and removes the minimum number of data points necessary to ensure the resulting surface satisfies task-specific constraints, such as maintaining a collision-free path to the goal.
- MPC Control: The refined surface is integrated into a Model Predictive Control (MPC) framework, specifically MPPI. The Gaussian Process's uncertainty is incorporated into the cost function, encouraging the robot to explore unknown regions of the environment.
Experimental Evaluation
The researchers evaluated COGIS across three distinct setups:
- Peg-in-Hole Tasks: Standard benchmarks (Peg-U, Peg-I, Peg-T) in PyBullet to test low-dimensional navigation.
- Simulated Cable Manipulation: A 16-DOF robot task in MuJoCo involving pulling a cable from under an occluding hook.
- Real-World Cable Manipulation: A physical experiment where a robot navigates an extension cord around a clamp and under a table to reach a power strip.
Key Results
- High Success Rates: In real-world cable tasks, COGIS achieved a 10/10 success rate, compared to just 1/10 for the TAMPC baseline.
- Importance of Refinement: In simulated cable tasks, COGIS succeeded in 23/30 trials. Removing the dataset refinement step dropped success to 17/30, highlighting the necessity of enforcing geometric constraints.
- Efficiency: Unlike reinforcement learning approaches, COGIS identifies and avoids obstacles within a single episode, demonstrating high sample efficiency.
Conclusion
COGIS demonstrates that enforcing geometric constraints on environment models is a powerful inductive bias for robot manipulation. By prioritizing "geometry over dynamics," the system achieves robustness to noise and occlusions. Future work will focus on relaxing the "contact-only" assumption for dynamics errors and exploring methods for the autonomous discovery of task-specific constraints in dynamic environments.
Summary updated