Manipulation Planning with Probabilistic Roadmaps

1. Manipulation Planning with Probabilistic Roadmaps Author: Thierry Simeon etc. Presenter: Yang Ruan

2. Introduction • Generally approach for a Robot move an object among some obstacles using continuous sets for modeling both the grasps and stable placements of the object.

3. Summary of Techniques • Two level probabilistic manipulation roadmap. • Random Loop Generator • Visibility-Probabilistic Roadmap • Rapidly-exploring Random Tree Planner

4. Challenge • Integrate the additional difficulty of planning the grasping and re-grasping operations to the path planning problem. • Continuous placements and grasps

5. Manipulation Planning • Configuration Space of the Robot CSroband Object CSobj • Full Configuration Space CS = CSrob× CSobj • CSfree are the admissible configurations in CS. • The CS corresponding to valid placement of M(movable object) is CP. • The CS corresponding to valid grasps configurations of M by R(Robot) is CG. • Transit Paths: where the robot moves alone while the object M remains stationary in a stable position.(foliation of CP) • Transfer Paths: where the robot moves while holding M with the same grasp .(foliation of CG)

6. Foliation • A foliation of an n-dimensional manifold M is an indexed family Lαof arc-wise connected m-dimensional submanifolds (m < n), called leaves of M, such that • – Lα ∩ Lα= ∅ if α = α • – ∪αLα = M • – every point in M has a local coordinate system such that n-m coordinates are constant

8. Theory • qi and qf in CG ∪ CP: There exists a manipulation path between qi and qfiff there exist two nodes (CG ∩ CP)i and (CG ∩ CP)f in MG(manipulation graph): • There exists a transit(or transfer) path from qi to some points in (CG ∩ CP)i . • There exists a transit(or transfer) path from some point in (CG ∩ CP)f to qf. • (CG ∩ CP)i and (CG ∩ CP)f belong to a same connected component of MG.

10. Approach: 2level-PMR • compute the connected components of CG ∩ CP; • Type1—a direct path from (g1, p1) to (g2, p2) lying inside CG ∩ CP is collision-free; • Type2a—a transfer path from (g1, p1) to (g1, p2) followed by a transit path from (g1, p2) to (g2, p2) are both collision-free; • Type2b—a transit path from (g1, p1) to (g2, p1) followed by a transfer path from (g2, p1) to (g2, p2) are both collision-free. • determine the connectivity of CG ∩ CP components using transit and transfer paths.

11. A Two-Level Probabilistic Manipulation Roadmap

12. Approach: Closed-Chain Systems • The set of continuous grasp: • Transformation Matrix: Tg(qgrasp) • CG corresponds to (qrob, qobc), qobj= G(qrob, qgrasp), so CG can be parameterized by (qrob, qgrasp). • The set of stable placements: • Transformation Matrix: Tg(qplace) • CP corresponds to qobj= P(qplace). • So CG∩CP can be parameterized by (qrob, qgrasp, qplace), satisfying the constraints G(qrob, qgrasp) = P(qplace).

13. Capturing CG ∩ CP Topogoly via Closed-Chain Systems

14. Planning techniques • Each planning problem has to be performed in a partially modified environment to re-use a recomputed static roadmap that is dynamically updated when solving the planning queries. • As for this case, this is solved point-to-point path planning problem: discrete grasps.

15. Connection Planner for Type1 Motion • Random Loop Generator: Reduce the cost of computing and connecting closure configurations • Transformed into a finite sequence of transit and transfer paths.

16. Roadmap construction • Visibility PRM: keeps the roadmap as small as possible

17. Connection Planner for Type2 Motions • First compute without the object, then add the object in. • Rapidly-exploring Random Tree(RRT) Planer.

18. Manipulation Planning Algorithm • computing CG ∩ CP connected components (Type1 adjacency) and linking them (Type2a-b adjacencies). • The function EXPAND_GRAPH performs one expansion step of MG. EXPAND_MANIP_GRAPH(MG) q ←NEW_CONFIG(MG) Type ←ADJACENCY_CHOICE(MG) nlinked comp. ←TEST_CONNECTIONS(MG, q, Type) if nlinked comp. ≠ 1 then ADD_NODE(q, MG, Type) UPDATE_GRAPH(MG) return TRUE else return FALSE

19. Node Generation • The continuous grasps and placement are defined by some transformation matrixes mentioned before: Tgi(qgrasp) and Tpj(qplace). And each couple(Tgi, Tpj) induces a closed-chain system. • A candidate node is generated as follows by the function NEW_CONFIG; it first randomly selects one couple (i, j ) of grasps and placement classes. The grasp and the stable placement of the movable object is then chosen by randomly sampling the parameters of vectors qgraspand qplace inside their variation interval. The candidate node N is generated when the sampled grasp and placement are collision-free and feasible for the couple(Tgi, Tpj).

20. Adjacency Selection • First conduct inside CG ∩ CP using Type1, and then determine connection between the components using Type2. • Function Adjacency_Choicedepdends on the size of the MG. If MG is small, then more likely use Type1, otherwise Type2. • This probability increases as the percentage of the coverage cov estimated by the fraction (1− 1/ntry). • Prob(Type1) = α.(1 − cov) and Prob(Type2) = 1 − Prob(Type1).

21. Edge Generation TEST_CONNECTIONS(MG, q, Type) nlinked comp.← 0 for k = 1 to N_COMP(MG) do if LINKED_TO_COMP(q, Ck , Type) then nlinked comp. = nlinked comp.+ 1 return nlinked comp. • Checked with Function Adjacency_Choice. • Type1 motion, using the closed chain planner, it is only possible with node computed for the same classes(Tgi, Tpj). • Type2 motion, stop checking as soon as there is a connection.

22. Solving Manipulation Queries • First, the start and goal configurations are connected to MG using the TEST_CONNECTIONS function called with a Type2 adjacency choice. • Second, transform the CG∩CP portions of the solution path into a finite sequence of transfer/transit paths. If the path is collision free, then use it. If both path are collide, use halfway along the CG∩CP. • Finally, solution is generated.

23. Performance • For solving the problem of Figure 1, the sliding motion allowing us to get the bar out the cage (see Figure 11) is obtained much more easily inside CG ∩ CP than the resulting sequence of transit/transfer paths

24. Influence of α • When α=0, the roadmap builder only consider collision-free transit and transfer paths to connect the random samples generated in CG ∩CP. When α becomes larger, the performance becomes better, however, will reduce when α>0.9 • In this more complicated example, it is qualitatively ad- vantageous to spend time on the connectively of CG ∩CP rather then checking connections with feasible paths.

25. Experiments

26. Results • With α = 0.9, it’s fast. • Most of time are spent on checking Transit/transfer paths. • Limit the number of such connection tests by first computing connected components inside CG ∩CP.

27. Conclusion • Solve the continuous formulation of the manipulation problem. • Use PRM to express the structure, transit and transfer can be connected. • Futurework: • Connection planner improvement, currently just pick and place. • Multiple movable object, currently just single movable object.

28. Thank you!