Manipulation Planning with Probabilistic Roadmaps

Author: Thierry Simeon etc.Presenter: Yang Ruan

IntroductionGenerally approach for a Robot move an

object among some obstacles using continuous sets for modeling both the grasps and stable placements of the object.

Summary of TechniquesTwo level probabilistic manipulation

roadmap.Random Loop GeneratorVisibility-Probabilistic RoadmapRapidly-exploring Random Tree Planner

ChallengeIntegrate the additional difficulty of planning

the grasping and re-grasping operations to the path planning problem.

Continuous placements and grasps

Manipulation PlanningConfiguration Space of the Robot CSrob and 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)

FoliationA 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

Theoryqi and qf in CG ∪ CP: There exists a

manipulation path between qi and qf iff there exist two nodes (CG ∩ CP)i and (CG ∩ CP)f in MG(manipulation graph):o There exists a transit(or transfer) path from qi to

some points in (CG ∩ CP)i .o There exists a transit(or transfer) path from some

point in (CG ∩ CP)f to qf.o (CG ∩ CP)i and (CG ∩ CP)f belong to a same

connected component of MG.

Approach: 2level-PMRcompute the connected components of CG ∩ CP;

o Type1—a direct path from (g1, p1) to (g2, p2) lying inside CG ∩ CP is collision-free;

o 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;

o 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.

A Two-Level Probabilistic Manipulation Roadmap

Approach: Closed-Chain SystemsThe set of continuous grasp:

o Transformation Matrix: Tg(qgrasp)o CG corresponds to (qrob, qobc), qobj = G(qrob,

qgrasp), so CG can be parameterized by (qrob, qgrasp).

The set of stable placements:o Transformation Matrix: Tg(qplace)o CP corresponds to qobj = P(qplace).

• So CG∩CP can be parameterized by (qrob, qgrasp, qplace), satisfying the constraints G(qrob, qgrasp) = P(qplace).

Capturing CG ∩ CP Topogoly via Closed-Chain Systems

Planning techniquesEach 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.

Connection Planner for Type1 MotionRandom Loop Generator: Reduce the cost of

computing and connecting closure configurations

Transformed into a finite sequence of transit and transfer paths.

Roadmap constructionVisibility PRM: keeps the roadmap as small

as possible

Connection Planner for Type2 MotionsFirst compute without the object,

then add the object in. Rapidly-exploring Random Tree(RRT)

Planer.

Manipulation Planning Algorithmcomputing 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 thenADD_NODE(q, MG, Type)UPDATE_GRAPH(MG)return TRUEelsereturn FALSE

Node GenerationThe 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 qgrasp and 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).

Adjacency SelectionFirst conduct inside CG ∩ CP using Type1, and

then determine connection between the components using Type2.

Function Adjacency_Choice depdends 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).

Edge GenerationTEST_CONNECTIONS(MG, q, Type)nlinked comp.← 0for k = 1 to N_COMP(MG) doif LINKED_TO_COMP(q, Ck , Type) thennlinked comp. = nlinked comp.+ 1return 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.

Solving Manipulation QueriesFirst, 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.

PerformanceFor 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

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.

Experiments

ResultsWith α = 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.

ConclusionSolve 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.

Thank you!