Barbara Frank, Cyrill Stachniss,

Nichola Abdo, Wolfram Burgard

University of Freiburg, Germany

Using Gaussian Process Regression for Efficient Motion Planning

in Environments with Deformable Objects

Motivation

How can we model the deformation properties of objects?

How can the robot consider this information when planning its motions?

Enable a robot to consider deformable obstacles when planning its motions

Planning with Deformation Cost

Estimating deformation is possible with finite element simulations

Manipulator planning: high-dimensional state space needs to be considered

Problem: too slow for online planning Challenge: fast estimation of the

deformation cost for manipulation robots Our approach:

Define a subset of possible motions and simulate the deformations before planning (training data)

Estimate the cost of new motions by regression

Planning Framework

Generate a Probabilistic roadmap (PRM) for the rigid part of the environment

Search for a path using and trade off path- and deformation cost:

Combination of motion planning and physically realistic deformation simulation:

Planning Framework

Generate a Probabilistic roadmap (PRM) for the rigid part of the environment

Search for a path using and trade off path- and deformation cost:

Euclidean distance inconfiguration space

Combination of motion planning and physically realistic deformation simulation:

Planning Framework

Generate a Probabilistic roadmap (PRM) for the rigid part of the environment

Search for a path using and trade off path- and deformation cost:

Euclidean distance inconfiguration space

Deformation simulation

Combination of motion planning and physically realistic deformation simulation:

Dynamic Simulation of Deformable Objects

Deformable modeling: 3D-tetrahedral model Finite Element Method

Simulation framework: Collision detection Collision response

Deformation simulations are costly and not suitable for online planning

Approximation & Assumptions

Our approach estimates the deformation cost based on training examples

Assumptions Obstacles are deformed but do not move Ignore interactions between different objects Consider only linear trajectories Deformation cost depend only on the arm

trajectory relative to an object and the material of the object

Deformation Cost Estimation

Given a set of sample trajectories and corresponding deformation cost values

Learn a predictive model

for estimating the deformation costof a new query trajectory

Trajectory parametrization: Starting point on a sphere End point on a sphere Traveled distance

Gaussian Processes (GPs)

GPs are a framework for non-parametric regression

Model the data points (here deformation cost) as jointly Gaussian

Predictive model for an input trajectory:

Provides a mean and a predictive variance

A covariance function models the influence of the data points on the query point

variance

meantrainingdata

Gaussian Processes (GPs)

Non-parametric model

Covariance function: squared exponential

… but the covariance function requires hyperparameters

Learning the hyperparameters by maximizing the likelihood of the training data

Popular: maximization via gradient methods

Problem: significant cost of learning the GP from data

Problem Decomposition

We need many samples to accurately approximate the deformation cost

Problem: GP learning has cubic runtime complexity in the number of samples due to matrix inversion

Approximation Store all samples in a KD-tree for efficient

organization and nearest neighbor queries Select only trajectory samples that are “close” to

build the GP

Nearest Neighbor Approximation

For each query trajectory, find the n closest neighbors from the training data (KD-tree)

Train a “local” GP Similar to setting for training data far

away from the query trajectory

Trajectory distance function:

Considering the Kinematic Chain

Simulation considers only the movement of the end-effector when generating samples

Consider the trajectories of different body parts (wrist, elbow, …)

Estimate the deformation cost of these trajectories using GP regression

Deformation cost of an edge in the roadmap: maximum of the individual trajectories

End-effector trajectory

Wrist trajectory

Evaluation: Prediction

Compare nearest-neighbor prediction (NN), GP with unit hyperparameters (GPStd), and GP with optimized hyperparameters (GPOpt)

Leave-one-out cross validation:

Predictive accuracy of deformation cost estimation:

Evaluation: Prediction

Compare nearest-neighbor prediction (NN), GP with unit hyperparameters (GPStd), and GP with optimized hyperparameters (GPOpt)

Cross validation D2 on D1:

Predictive accuracy of deformation cost estimation:

Evaluation: Performance

Preprocessing simulations

Roadmap computation

Answering path queries

Planner with integrated simulation

-

307min(267min

simulation)

10min(9.7min

simulation)

Planner with our GP-based

estimation~ 36h

42min(2min GP-

evaluation)

5.3s (1.8s GP-

evaluation)

Long preprocessing, but only once per object Independent of the environment Speedup of 2 orders of magnitude during roadmap

computation + query time

Runtime requirements compared to a planner with integrated simulation:

Motion Planning Example

Trade-off between path cost and deformation cost

Shortest path

Motion Planning Example

Trade-off between path cost and deformation cost

Shortest path

Related Work

Planning for deformable robots: [Kavraki et al. 98/00, Bayazit et al. 02, Gayle et al. 05]

Planning in completely deformable environments: [Rodriguez et al. 06, Patil et al. 11]

Application: medical simulation [Maris et al. 10, Alterovitz et al. 09]

GP NN approximation for terrain modeling [Vasudevan et al. 09]

Conclusion

Novel approach to manipulator motion planning considering deformable obstacles

Efficient estimation of the deformation cost along a trajectory using Gaussian process regression

GP training using a deformation simulation based on finite element method

Experiments illustrate an accurate cost estimation and online planning capabilities

Thanks for Your Attention!