Sigma Hulls for Gaussian Belief Space Planning for Imprecise - - PowerPoint PPT Presentation

▶
sigma hulls for gaussian belief space planning for
SMART_READER_LITE
LIVE PREVIEW

Sigma Hulls for Gaussian Belief Space Planning for Imprecise - - PowerPoint PPT Presentation

Feb 29, 2024 178 likes •564 views

Sigma Hulls for Gaussian Belief Space Planning for Imprecise Articulated Robots amid Obstacles Alex Lee , Yan Duan, Sachin Patil, John Schulman, Zoe McCarthy, Jur van den Berg*, Ken Goldberg and Pieter Abbeel UC Berkeley, *University of Utah

slide-1
SLIDE 1

Sigma Hulls for Gaussian Belief Space Planning for Imprecise Articulated Robots amid Obstacles

Alex Lee, Yan Duan, Sachin Patil, John Schulman, Zoe McCarthy, Jur van den Berg*, Ken Goldberg and Pieter Abbeel

UC Berkeley, *University of Utah

slide-2
SLIDE 2

Facilitate reliable operation of cost-effective robots that use:

 Imprecise actuation mechanisms – serial elastic actuators,

cables

 Inaccurate sensors – encoders, gyros, accelerometers

Motivation

Presenter: Alex Lee (UC Berkeley)

slide-3
SLIDE 3

 Planning under motion and sensing uncertainty is a POMDP in

general

 Intractable in general  Compute locally optimal solutions

 Bry et al (ICRA 2011), Li et al (IJC 2007), van den Berg et al (IJRR

2011), van den Berg et al (IJRR 2012), Platt et al (RSS 2010)

Prior Work on Gaussian Belief Space Planning

Presenter: Alex Lee (UC Berkeley)

slide-4
SLIDE 4

Gaussian Belief Space Planning

Presenter: Alex Lee (UC Berkeley)

[Example from Platt, T edrake, Kaelbling, Lozano-Perez, 2010]

start goal

Problem Setup

slide-5
SLIDE 5

State space plan

Gaussian Belief Space Planning

Presenter: Alex Lee (UC Berkeley)

[Example from Platt, T edrake, Kaelbling, Lozano-Perez, 2010]

slide-6
SLIDE 6

State space plan

Gaussian Belief Space Planning

Presenter: Alex Lee (UC Berkeley)

[Example from Platt, T edrake, Kaelbling, Lozano-Perez, 2010] Belief space plan

slide-7
SLIDE 7

 Gaussian belief state in joint space: 𝑐𝑢 =

𝜈𝑢 Σ𝑢

Gaussian Belief Space Planning using Trajectory Optimization

mean square root of covariance

Presenter: Alex Lee (UC Berkeley)

slide-8
SLIDE 8

 Gaussian belief state in joint space: 𝑐𝑢 =

𝜈𝑢 Σ𝑢

 Optimization problem:

Gaussian Belief Space Planning using Trajectory Optimization

mean square root of covariance

Unscented Kalman Filter dynamics Reach desired end-effector pose Control inputs are feasible

Presenter: Alex Lee (UC Berkeley)

𝑐𝑢+1 = belief_dynamics 𝑐𝑢, 𝑣𝑢 𝜈𝑈 = goal 𝑣𝑢 ∈ 𝑉 min 𝐷 𝑐0, … , 𝑐𝑈, 𝑣0, … , 𝑣𝑈−1

  • s. t. ∀ 𝑢 = 1, … , 𝑈
slide-9
SLIDE 9

 Gaussian belief state in joint space: 𝑐𝑢 =

𝜈𝑢 Σ𝑢

 Optimization problem:  Non-convex optimization – Can be solved using sequential

quadratic programming (SQP)

Gaussian Belief Space Planning using Trajectory Optimization

mean square root of covariance

Unscented Kalman Filter dynamics Reach desired end-effector pose Control inputs are feasible

Presenter: Alex Lee (UC Berkeley)

𝑐𝑢+1 = belief_dynamics 𝑐𝑢, 𝑣𝑢 𝜈𝑈 = goal 𝑣𝑢 ∈ 𝑉 min 𝐷 𝑐0, … , 𝑐𝑈, 𝑣0, … , 𝑣𝑈−1

  • s. t. ∀ 𝑢 = 1, … , 𝑈
slide-10
SLIDE 10

 Want to include probabilistic collision avoidance constraints

Prior Work on Gaussian Belief Space Planning

Presenter: Alex Lee (UC Berkeley)

slide-11
SLIDE 11

 Want to include probabilistic collision avoidance constraints  Prior work approximates robot geometry as point/spheres

Prior Work on Gaussian Belief Space Planning

Presenter: Alex Lee (UC Berkeley)

𝑦1 𝑦2 𝑦1 𝑦2 𝑦1 𝑦2

slide-12
SLIDE 12

 Want to include probabilistic collision avoidance constraints  Prior work approximates robot geometry as point/spheres  How do you formulate the constraint for a robot link?

Prior Work on Gaussian Belief Space Planning

Presenter: Alex Lee (UC Berkeley)

𝑦1 𝑦2 𝑦1 𝑦2 𝑦1 𝑦2 𝜄1 𝜄2 𝜄1 𝜄2

?

slide-13
SLIDE 13

Main Contribution: Incorporation of Collision Avoidance Constraints under Uncertainty through Sigma Hulls

Presenter: Alex Lee (UC Berkeley)

𝜄1 𝜄2 𝜄1 𝜄2 𝜄1 𝜄2 𝑦 = 𝜄1 𝜄2 𝒴 = 𝑦 𝑦 𝑦 𝑦 𝑦 + 𝜇 0 Σ − Σ

slide-14
SLIDE 14

Main Contribution: Incorporation of Collision Avoidance Constraints under Uncertainty through Sigma Hulls

Presenter: Alex Lee (UC Berkeley)

𝜄1 𝜄2 𝜄1 𝜄2 𝜄1 𝜄2 𝑦 = 𝜄1 𝜄2 𝒴 = 𝑦 𝑦 𝑦 𝑦 𝑦 + 𝜇 0 Σ − Σ

slide-15
SLIDE 15

Main Contribution: Incorporation of Collision Avoidance Constraints under Uncertainty through Sigma Hulls

Presenter: Alex Lee (UC Berkeley)

𝜄1 𝜄2 𝜄1 𝜄2 𝜄1 𝜄2 𝑦 = 𝜄1 𝜄2 𝒴 = 𝑦 𝑦 𝑦 𝑦 𝑦 + 𝜇 0 Σ − Σ

slide-16
SLIDE 16

Main Contribution: Incorporation of Collision Avoidance Constraints under Uncertainty through Sigma Hulls

Presenter: Alex Lee (UC Berkeley)

𝜄1 𝜄2 𝜄1 𝜄2 𝜄1 𝜄2 𝑦 = 𝜄1 𝜄2 𝒴 = 𝑦 𝑦 𝑦 𝑦 𝑦 + 𝜇 0 Σ − Σ Sigma hull of link 1

slide-17
SLIDE 17

Sigma hull: Convex hull of a robot link transformed (in joint space) according to sigma points Main Contribution: Incorporation of Collision Avoidance Constraints under Uncertainty through Sigma Hulls

Presenter: Alex Lee (UC Berkeley)

𝜄1 𝜄2 𝜄1 𝜄2 𝜄1 𝜄2 𝑦 = 𝜄1 𝜄2 𝒴 = 𝑦 𝑦 𝑦 𝑦 𝑦 + 𝜇 0 Σ − Σ Sigma hull of link 1 Sigma hull of link 2

slide-18
SLIDE 18

Signed Distance

Presenter: Alex Lee (UC Berkeley)

Consider signed distance between obstacle 𝑃 and sigma hull 𝒝𝑗,𝑢 of the 𝑗-th link at time 𝑢

𝑃 𝑃 𝒝𝑗,𝑢 𝒝𝑗,𝑢

𝒝𝑗,𝑢 = sigmahull(link𝑗,𝑢)

slide-19
SLIDE 19

 Use convex-convex collision detection (GJK and EPA algorithm)

 Computes signed distance of convex hull efficiently

Collision Avoidance Constraint: Signed Distance

slide-20
SLIDE 20

 Use convex-convex collision detection (GJK and EPA algorithm)

 Computes signed distance of convex hull efficiently

 Sigma hulls should stay at least distance 𝑒safe from other objects

Collision Avoidance Constraint: Signed Distance

Presenter: Alex Lee (UC Berkeley)

𝑒safe Signed Distance

∀ times 𝑢, ∀ links 𝑗, ∀ obstacles 𝑃 sd 𝒝𝑗,𝑢, 𝑃 ≥ 𝑒safe

slide-21
SLIDE 21

 Use convex-convex collision detection (GJK and EPA algorithm)

 Computes signed distance of convex hull efficiently

 Sigma hulls should stay at least distance 𝑒safe from other objects  Use analytical gradients for the signed distance

Collision Avoidance Constraint: Signed Distance

Presenter: Alex Lee (UC Berkeley)

𝑒safe Signed Distance

Non-convex! ∀ times 𝑢, ∀ links 𝑗, ∀ obstacles 𝑃 sd 𝒝𝑗,𝑢, 𝑃 ≥ 𝑒safe

slide-22
SLIDE 22

 Discrete collision avoidance can lead to trajectories that collide

with obstacles in between time steps

Continuous Collision Avoidance Constraint

Presenter: Alex Lee (UC Berkeley)

slide-23
SLIDE 23

 Discrete collision avoidance can lead to trajectories that collide

with obstacles in between time steps

 Use convex hull of sigma hulls between consecutive time steps

Continuous Collision Avoidance Constraint

Presenter: Alex Lee (UC Berkeley)

sd convhull(𝒝𝑗,𝑢, 𝒝𝑗,𝑢+1), 𝑃 ≥ 𝑒safe ∀ 𝑢, 𝑗, 𝑃

slide-24
SLIDE 24

 Discrete collision avoidance can lead to trajectories that collide

with obstacles in between time steps

 Use convex hull of sigma hulls between consecutive time steps  Advantages:

 Solutions are collision-free

in between time-steps

 Discretized trajectory can

have less time-steps

Continuous Collision Avoidance Constraint

Presenter: Alex Lee (UC Berkeley)

sd convhull(𝒝𝑗,𝑢, 𝒝𝑗,𝑢+1), 𝑃 ≥ 𝑒safe ∀ 𝑢, 𝑗, 𝑃

slide-25
SLIDE 25

 Gaussian belief state in joint space: 𝑐𝑢 =

𝑦𝑢 Σ𝑢

 Optimization problem:  Non-convex optimization – Can be solved using sequential

quadratic programming (SQP)

Gaussian Belief Space Planning using Trajectory Optimization

mean square root of covariance

Unscented Kalman Filter dynamics Reach desired end-effector pose Control inputs are feasible

Presenter: Alex Lee (UC Berkeley)

𝑐𝑢+1 = belief_dynamics 𝑐𝑢, 𝑣𝑢 pose 𝑦𝑈 = target_pose 𝑣𝑢 ∈ 𝑉 min 𝐷 𝑐0, … , 𝑐𝑈, 𝑣0, … , 𝑣𝑈−1

  • s. t. ∀𝑢 = 1, … , 𝑈

Probabilistic collision avoidance

?

slide-26
SLIDE 26

 Gaussian belief state in joint space: 𝑐𝑢 =

𝑦𝑢 Σ𝑢

 Optimization problem:  Non-convex optimization – Can be solved using sequential

quadratic programming (SQP)

Gaussian Belief Space Planning using Trajectory Optimization

mean square root of covariance

Unscented Kalman Filter dynamics Reach desired end-effector pose Control inputs are feasible

Presenter: Alex Lee (UC Berkeley)

𝑐𝑢+1 = belief_dynamics 𝑐𝑢, 𝑣𝑢 pose 𝑦𝑈 = target_pose 𝑣𝑢 ∈ 𝑉 min 𝐷 𝑐0, … , 𝑐𝑈, 𝑣0, … , 𝑣𝑈−1

  • s. t. ∀𝑢 = 1, … , 𝑈

sd sigma_hull𝑗(𝑐𝑢), 𝑃 ≥ 𝑒safe ∀ 𝑗, 𝑃 Probabilistic collision avoidance

slide-27
SLIDE 27

 During execution, re-plan after every belief state update  Update the belief state based on the actual observation  Effective feedback control, provided one can re-plan

sufficiently fast

Model Predictive Control (MPC)

Presenter: Alex Lee (UC Berkeley)

slide-28
SLIDE 28

 Problem setup

Example: 4-DOF planar robot

Presenter: Alex Lee (UC Berkeley)

slide-29
SLIDE 29

State-space trajectory

Example: 4-DOF planar robot

Presenter: Alex Lee (UC Berkeley)

slide-30
SLIDE 30

1-standard deviation belief space trajectory

Example: 4-DOF planar robot

Presenter: Alex Lee (UC Berkeley)

slide-31
SLIDE 31

4-standard deviation belief space trajectory

Example: 4-DOF planar robot

Presenter: Alex Lee (UC Berkeley)

slide-32
SLIDE 32

 Open-loop execution  Feedback linear policy  Re-planning (MPC)

Experiments: 4-DOF planar robot

Presenter: Alex Lee (UC Berkeley)

slide-33
SLIDE 33

Mean distance from target

Experiments: 4-DOF planar robot

Presenter: Alex Lee (UC Berkeley)

slide-34
SLIDE 34

Example: 7-DOF articulated robot

Presenter: Alex Lee (UC Berkeley)

State space trajectory 7 dimensions 1.9 secs Belief space trajectory 35 dimensions 8.2 secs

slide-35
SLIDE 35

Extensions

 Planning in uncertain environments  Multi-modal belief spaces  Physical experiments with the Raven surgical robot

Presenter: Alex Lee (UC Berkeley)

slide-36
SLIDE 36

 Efficient trajectory optimization in Gaussian

belief spaces to reduce task uncertainty

 Prior work approximates robot geometry as a

point or a single sphere

 Pose collision constraints using signed distance

between sigma hulls of robot links and obstacles

 Sigma hulls never explicitly computed – use fast

convex collision detection and analytical gradients

 Iterative re-planning in belief space (MPC)

Conclusions

Presenter: Alex Lee (UC Berkeley)

slide-37
SLIDE 37

 Code available upon request  Contact: alexlee_gk@berkeley.edu

Thank You

Presenter: Alex Lee (UC Berkeley)