Robust Belief-based Execution of Manipulation Programs

1. Robust Belief-based Execution ofManipulation Programs Kaijen Hsiao Tomás Lozano-Pérez Leslie Pack Kaelbling MIT CSAIL

2. Achieving Goals under Uncertainty • Two kinds of uncertainty: • current state: • need to plan in information space • results of future actions: • search branches on outcomes as well as actions • Choice of action must be dependent on current information state

3. Discrete POMDP Formulation • states • actions • observations • transition model • observation model • reward

4. POMDP Controller Controller • State estimation is discrete Bayesian filter • Policy maps belief states to actions belief  SE sensing action Environment

5. Action selection in POMDPs • Off-line optimal policy generation • Intractable for large spaces • On-line search: finite-depth expansion of belief-space tree from current belief state to select single action • Tractable in broad subclass of problems

6. Challenges for action selection • Continuous state spaces • Requirement to select action for any belief state • Long horizon • Action branching factor • Outcome branching factor • Computationally complex observation and transition models

7. Grasping in uncluttered environments • Points of leverage: • Robot pose is approximately observable • Robot dynamics are nearly deterministic • Bounded uncertainty over unobserved object parameters • Room to maneuver

8. Online belief-space search • Continuous state space: discretize object state space

9. Discretize object configuration space workspace configuration space belief state

10. Online belief-space search • Continuous state space: discretize object state space • Action for any belief: search forward from current belief state

11. Search forward from current belief • Low entropy belief states enable reliable grasp • Use entropy as static evaluation function at leaves • Actions can be useful for information gathering

12. Online belief-space search • Continuous state space: discretize object state space • Action for any belief: search forward from current belief state • Long horizon: use temporally extended actions

13. Use temporally extended actions • Primitive actions Entire trajectories • Reduce horizon Observations at end

14. Online belief-space search • Continuous state space: discretize object state space • Action for any belief: search forward from current belief state • Long horizon: use temporally extended actions • Large action branching factor: parameterize small set of action types by current belief

15. Parameterize actions with belief • Actions are entire world-relative trajectories • In current belief state, • execute with respect to most likely object configuration • terminate on contact or end of trajectory

16. Online belief-space search • Continuous state space: discretize object state space • Action for any belief: search forward from current belief state • Long horizon: use temporally extended actions • Large action branching factor: parameterize small set of action types by current belief • Computationally complex observation and transition models: precompute models

17. Precompute models • Execute WRT • with respect to estimated state e • in world state w • Expected observation,transition • Based on geometric simulation

18. Online belief-space search • Continuous state space: discretize object state space • Action for any belief: search forward from current belief state • Long horizon: use temporally extended actions • Large action branching factor: parameterize small set of action types by current belief • Computationally complex observation and transition models: precompute models • Large observation branching factor: canonicalize observations for each discrete state and action

19. Canonicalize observations • Any (e, w) pair with same relative transformation has same world-relative outcomes and observations • Only sample for one e with w varying within initial range of uncertainty • Cluster observations and represent each bin of object configurations by a single representative one • Only branch on canonical observations

20. Algorithm • Off-line: • plan WRTs for grasping and info gathering • compute models • On-line: • while current belief state doesn’t satisfy goal • compute expected info gain of each WRT • execute best WRT until termination • use observation to update current belief • return to initial pose • execute final grasp trajectory

21. Application to grasping with simulated robot arm • Initial conditions (ultimately from vision) • Object shape is roughly known (contacted vertices should be within ~1 cm of actual positions) • Object is on table and pose (x, y, rotation) is roughly known (center of mass std ~5 cm, 30 deg) • Achieve specific grasp of object

22. Observations • Fingertips: 6-axis force/torque sensors • position • normal • Additional contact sensors: • just contact • Swept non-colliding path rules out poses that would have generated contact

23. Grasping a Box Most likely robot-relative position Where it actually is

24. Initial belief state

25. Summed over theta

26. Tried to move down; finger hit corner

27. Probability of contact observation at each location

28. Updated belief

29. Re-centered

30. Trying again, with new belief Back up Try again

31. Final state and observation Observation probabilities Grasp

32. Updated belief state: Success! Goal: variance < 1 cm x, 15 cm y, 6 deg theta

33. What if Y coord of grasp matters?

34. Need explicit information gathering

35. Simulation Experiments • Methods tested: • Single open-loop execution of goal-achieving WRT with respect to the most likely state • Repeated execution of goal-achieving WRT with respect to the most likely state • Online selection of information-gathering and goal-achieving grasps (1-step lookahead)

36. Box experiments • Allowed variation in goal grasp: 1 cm, 1 cm, 5 deg • Initial uncertainty: 5 cm, 5 cm, 30 deg

37. Cup experiments

38. Cup experiments • Goal 1 cm x, 1 cm y, rotation doesn’t matter (no info-grasps used) • Start uncertainty 30 deg theta (x,y varies) Increasing uncertainty

39. Grasping a Brita Pitcher Target grasp: Put one finger through the handle and grasp

40. Brita Pitcher experiments

41. Brita Pitcher results Increasing uncertainty

42. Other recent probabilistic approaches to manipulation • Off-line POMDP solution for grasping (Hsiao et al. 2007) • Bayesian state estimation using tactile sensors to locate object before grasping (Petrovskaya et al. 2006) • Finding a fixed trajectory that is most likely to succeed under uncertainty (Alterovitz et al. 2007, Burns and Brock 2007)

43. The End.

44. Timing For Brita Pitcher • (2.16 GHz processor, 3.24 GB RAM running Python, times in seconds)

45. Number of Actions Used

46. Creating Information-gain Trajectories • Trajectory generation • Generate endpoints, use randomized planner (such as OpenRAVE) to find nominal collision-free path • Sweep through entire workspace • Choose a small set based on information gain from start uncertainty