A Probabilistic Framework for Constrained Manipulations and Task and Motion Planning under Uncertainty

Jung-Su Ha, Danny Driess, Marc ToussaintICRA 2020 · OpenReview

Introduction

Robotic manipulation planning is inherently challenging due to high-dimensional continuous state spaces and the discontinuous dynamics involved in making and breaking physical contact. While Logic-Geometric Programming (LGP) has proven effective by combining discrete logic with continuous trajectory optimization, standard implementations are purely deterministic. They assume perfect execution and prioritize path efficiency over robustness, often leading to "brittle" plans that fail under real-world disturbances.

This research extends LGP into stochastic domains, framing motion planning as a problem of probabilistic inference. By accounting for uncertainty, the framework evaluates plans based on their robustness to noise. This approach leads to emergent behaviors such as "contact exploitation"—where a robot uses the environment to stabilize itself—and enables a reactive controller capable of switching between high-level strategies dynamically.

Methodology: Control-Inference Duality

The core of this framework leverages the control-inference duality, which maps Stochastic Optimal Control (SOC) problems to Kullback-Leibler (KL) divergence minimization. In this view, finding an optimal control policy is equivalent to inferring a posterior path distribution.

Probabilistic LGP as a Mixture of Gaussians

The optimal path distribution is approximated as a Gaussian mixture model (GMM). Each component in the mixture corresponds to a specific "logic skeleton" (a sequence of discrete actions like grasp or push). For each skeleton, the authors employ k-order constrained dynamic programming (KODP) to optimize the trajectory. A Laplace approximation is then formed using the gradient and Hessian from this optimization, effectively fitting a Gaussian distribution around the optimal path.

Cost Decomposition

The framework determines the weight of each plan in the mixture through a two-part cost function:

  1. Deterministic Path Cost: The traditional measure of energy or time efficiency.
  2. Stochastic Cost: A measure of the high-gain feedback control required to maintain the robot on its path under uncertainty.
The framework approximates optimal path distributions as a mixture of Gaussians, allowing it to evaluate multiple logic skeletons and their robustness to disturbances.

Key Insights and Emergent Behaviors

Stochastic Cost and Robustness

By explicitly penalizing plans that require aggressive feedback to survive disturbances, the framework naturally favors stable strategies. For example, in box-pushing tasks, the system identifies that a two-finger push is vastly more robust to actuator noise than a single-finger push, even if the latter is theoretically faster.

Emergent Contact Exploitation

A significant finding is the mathematical justification for contact exploitation. The framework recognizes that adding physical constraints—such as resting an elbow on a table during a precision task—reduces the covariance of the uncontrolled path distribution. The planner actively chooses to exploit these environmental contacts to "reject" disturbances, mirroring human strategies for precision work.

Experimental Results and Reactive Control

The authors evaluated the framework in simulated environments requiring complex manipulation and task switching.

Reactive Control (The Banana Problem)

In a scenario where a robot must climb boxes to reach a target, the framework utilizes a composite reactive controller. During execution, the robot updates the probability weights of each skeleton based on its current state. If a large disturbance pushes the robot away from its primary plan, the controller can seamlessly switch to a backup strategy (e.g., using a different box) rather than fighting to return to the original, now-suboptimal path.

A sequence showing the robot executing a manipulation task, pushing a box toward a target zone while maintaining stability.

Performance Metrics

In "elbow-on-table" reaching tasks, the framework successfully balanced deterministic and stochastic costs, choosing a strategy with moderate path cost but significantly lower uncertainty. In box-pushing experiments, the probabilistic approach reduced RMS error from 0.3727 (single-finger) to 0.0952 (two-finger) under simulated disturbances.

Limitations and Future Directions

While the framework enables fluid plan switching, the "blending" of control inputs can sometimes lead to smoothing artifacts, such as a robot hovering near a surface rather than making firm contact. Additionally, the combinatorial explosion of possible logic skeletons remains a challenge for scaling to more complex tasks. Future work aims to integrate deep learning architectures to prune the skeleton space and utilize LQR-trees for more efficient reactive evaluation.

Summary updated

Abstract

Logic-Geometric Programming (LGP) is a powerful motion and manipulation planning framework, which represents hierarchical structure using logic rules that describe discrete aspects of problems, e.g., touch, grasp, hit, or push, and solves the resulting smooth trajectory optimization. The expressive power of logic allows LGP for handling complex, large-scale sequential manipulation and tool-use planning problems. In this paper, we extend the LGP formulation to stochastic domains. Based on the control-inference duality, we interpret LGP in a stochastic domain as fitting a mixture of Gaussians to the posterior path distribution, where each logic pro le defines a single Gaussian path distribution. The proposed framework enables a robot to prioritize various interaction modes and to acquire interesting behaviors such as contact exploitation for uncertainty reduction, eventually providing a composite control scheme that is reactive to disturbance.
BibTeX
@inproceedings{ha2020probabilistic,
  author = {Jung-Su Ha and Danny Driess and Marc Toussaint},
  title = {{A Probabilistic Framework for Constrained Manipulations and Task and Motion Planning under Uncertainty}},
  booktitle = {IEEE International Conference on Robotics and Automation},
  year = {2020},
  url = {https://openreview.net/forum?id=wOjF2mF0FBi},
}

Loading related content...

Figure