Probabilistic Inference in Planning for Partially Observable Long Horizon Problems

Authors: Alphonsus Adu-Bredu, Nikhil Devraj, Pin-Han Lin, Zhen Zeng, Odest Chadwicke Jenkins · IROS 2021

Licences

Open Access
Paper: CC BY 4.0

Everything below draws on the full paper under its CC BY 4.0 licence; its figure is credited to the authors.

TL;DR

SHY-COBRA grounds a symbolic plan's grasps, poses and trajectories by belief propagation over a constraint graph seeded with noisy beliefs, then executes, observes and replans.

Why it matters

Real robots face noise both in where objects are and in their own joint states, and a planner that treats only one of these will miss grasps. Representing every continuous parameter as a sample-based belief lets any uncertainty model enter the plan, which is what a mobile manipulator needs for multi-step household tasks.

Problem

Long-horizon service tasks such as cooking need integrated task and motion planning (TAMP), but most TAMP methods assume the state is fully known and actions are deterministic. Methods that handle partial observability usually solve the hybrid constraint problem by sampling or optimisation and reduce the belief to its most likely value. The closest prior method, SS-Replan, models uncertainty only in object poses.

Proposed solution

A symbolic planner (Fast Downward) produces a plan skeleton; its free parameters (robot configurations, object poses, grasps, arm trajectories) become variable nodes in a factor graph whose factors are constraints such as kinematic feasibility, collision-free motion, stable grasp and stable placement. Variables are initialised with samples from the robot's noisy belief, and max-product Pull Message Passing Nonparametric Belief Propagation (PMPNBP) infers a jointly satisfying assignment. The robot executes each action, updates its belief from observation, and rebuilds the graph and replans if the effect is not the expected one.

Key insights

Grounding a plan skeleton can be cast as inference on a factor graph, with constraints as factors and action parameters as variables.
Nonparametric belief propagation keeps the full sample-based belief rather than collapsing it to a maximum-likelihood estimate, so arbitrary uncertainty distributions can be used.
Modelling joint-configuration noise as well as object-pose noise reduces missed picks and placements compared with a planner that models pose noise only.
Planning cost of the method grows with the magnitude of estimation noise.

Main results

Retrieve task: 9.37 ± 5.25 s planning time and 1.33 ± 1.52 errors for SHY-COBRA against 16.61 ± 5.68 s and 8.67 ± 4.93 errors for SS-Replan*.
Wash task: 15.72 ± 6.91 s and 2.67 ± 3.06 errors against 21.32 ± 8.53 s and 7.00 ± 3.60 errors.
Cook task: 25.15 ± 5.16 s and 3.50 ± 2.12 errors against 29.28 ± 13.61 s and 6.33 ± 0.58 errors.
Serve-meal task: 37.24 ± 10.04 s and 7.00 ± 2.83 errors against 52.75 ± 16.82 s and 15.5 ± 0.71 errors.
Results are over 12 randomly generated problems per task, with object-pose noise of 10 cm and joint-configuration noise of 0.25 rad standard deviation, 100 samples each and 10 PMPNBP iterations per planning cycle.

Limitations

The authors state that the method's time complexity grows with the magnitude of noise in the estimates (Fig. 6).
All experiments are in PyBullet simulation with a simulated Digit robot; there is no physical-robot test.
The experiments assume zero-mean Gaussian noise on pose and joint estimates, although the method is stated to be agnostic to the distribution type.
The plan skeleton comes from a separate symbolic planner, and the only baseline is the authors' adaptation SS-Replan* of SS-Replan.

Field impact

Winner of the IROS 2021 Best Paper Award on Mobile Manipulation, cited 12 times. The paper names no code or data release.

Abstract

For autonomous service robots to successfully perform long horizon tasks in the real world, they must act intelligently in partially observable environments. Most Task and Motion Planning approaches assume full observability of their state space, making them ineffective in stochastic and partially observable domains that reflect the uncertainties in the real world. We propose an online planning and execution approach for performing long horizon tasks in partially observable domains. Given the robot's belief and a plan skeleton composed of symbolic actions, our approach grounds each symbolic action by inferring continuous action parameters needed to execute the plan successfully. To achieve this, we formulate the problem of joint inference of action parameters as a Hybrid Constraint Satisfaction Problem (H-CSP) and solve the H-CSP using Belief Propagation. The robot executes the resulting parameterized actions, updates its belief of the world and replans when necessary. Our approach is able to efficiently solve partially observable tasks in a realistic kitchen simulation environment. Our approach outperformed an adaptation of the state-of-the-art method across our experiments.

Open Source Robotics