Motion Planning

Department of InformaticsIntelligent Autonomous Systems Technische Universitt Mnchen

Technical Cognitive Systems

Michael Beetz

Intelligent Autonomous SystemsTechnische Universitat Munchen

Summer Term 2012

Part XI

Motion Planning


Outline

Motion Planning

Motion Planning

Michael BeetzSummer Term 2012

Technical Cognitive Systems3


Outline

Motion Planning

Motion Planning




Motion Planning

Motion Planning




Motion Planning

http://planning.cs.uiuc.edu/

Motion Planning



http://planning.cs.uiuc.edu/


The basic Path Planning Problem

Given obstacles, a robot, and its motion capabilities compute collision-free robotmotions from the start to goal.

Motion Planning




Geometric Models

The robot and obstacles live in a world or workspace W.Usually, W = R2 or W = R3.The obstacle region O W is a closed set.The robot A(q) W is a closed set (placed at configuration q).

Can it be obtained automatically or with little processing? What is the complexity of the representation? Can collision queries be efficiently resolved? Can a solid or surface be easily inferred?

Motion Planning




Geometric Models: Linear Primitives

Motion Planning




Geometric Models: Semi-Algebraic Sets

Consider primitives of the form:

Hi = {(x , y , z) W|fi (x , y , z) < 0},

which is a half-space if fi is linear. Now let fi be any polynomial, such asf (x , y) = x2 + y2 1. Obstacles can be formed from finite intersections:

O = H1 H2 H3 H4.

and from finite unions of those:

O = H1 H2 H3 H4.

O could then become any semi-algebraic set.Motion Planning




Geometric Models: Polygon Soups and Pointclouds

Whats inside?

Motion Planning




Configuration Space: Example

All configurations of our robot lie on a manifold, here a torus.

Motion Planning




Excursion: Flat Manifolds: Cylinder

Start with an open square (0, 1)2) R2

Let (x , y) denote a point on the manifold.Include the x = 0 points and define equivalence relation :

(0, y) (1, y)

for all y (0, 1).

Motion Planning




Excursion: Flat Moebius Band

Change the equivalence relation :

(0, y) (1, 1 y)

for all y (0, 1).Motion Planning




Excursion: Other Flat Manifolds

Motion Planning




Configuration Space: Obstacle Region

Given world W, a closed obstacle O W, a closed robot A, and configurationspace C.Let A(q) W denote the placement of the robot into configuration q.The obstacle region Cobs in C is

Cobs = {q C|A(q) O 6= },

which is a closed set. The free space Cfree is an open subset of C:

Cfree = C \ Cobs

We want to keep the configuration in Cfree at all times!

Motion Planning




Obstacle Region: Example

For the case of two links, C = S1 S1, the obstacle region already becomesstrange and complicated!

Motion Planning




Obstacle Region: Minkowski Sum

Also known as Convolution.Motion Planning




Obstacle Region: Polygonal Obstacles

Motion Planning




Configuration Space: Planning Problem Revisited

Given robot A and obstacle models O, C-space C and qI , qG Cfree .

Compute a path : [0, 1] Cfree so that (0) = qI and (1) = qG .

Motion Planning




Combinatorial vs. Sampling-Based planning: the two families

Two families of motion planning algorithms exist:

Combinatorial Planning (exact planning)

Sampling-Based-Planning (probabilistic. randomized planning)

Motion Planning




Completeness Notions

A planning algorithm may be:

Complete: If a solution exists, it finds one; otherwise, it reports failure. Semi-complete: If a solution exists, it finds one; otherwise, it may run

forever.

Resolution complete: If a solution exists at a given resolution, it findsone; otherwise, it terminates and reports no solution within this resolutionexists.

Probabilistically complete: If a solution exists, the probability that it isfound tends to one as the number of iterations tends to infinity.

Compare with decidability/computability!

Motion Planning




Combinatorial Planning:

Mostly developed in the 1980s Explicit construction of Cobs Influence from computational geometry and computational real algebraic

geometry

All algorithms are complete Usuall produce a roadmap in Cfree Extremely efficient for low-dimensional problems but dont scale well for

higher dimensional problems

Some are difficult to implement (numerical issues)

Motion Planning




Combinatorial Planning: Roadmaps

Combinatorial Planning Methods produce a topological graph G: Each vertex is a configuration q Cfree Each edge is a path : [0, 1] Cfree for which (0) and (1) are vertices.

A roadmap is a topological graph G with two properties: Accessibility: From anywhere in Cfree it is trivial to compute a path that

reaches at least one point along any edge in G. Connectivity-preserving: If there exists a path through Cfree from qI toqG , then there must also exist one that travels through G.

Motion Planning




Combinatorial Planning in a Polygonal Obstacle Region

Assume that Cobs (and Cfree) are piecewise linear.Could be a point robot among polygonal obstacles.Could be a polygonal, translating robot among polygonal obstacles.This methods tend to extend well to disc robots (e.g. roomba).

Use clever datastructures to encode vertices, edges, regions.Example: Doubly connected edge list.

Motion Planning




Combinatorial Planning in a Polygonal Obstacle Region

We consider four methods:

Trapezoidal Decomposition Triangulation Maximum Clearance Roadmap (retraction method) Shortest-path roadmap (reduced visibility graph)

Motion Planning




Combinatorial Planning Algorithms: Trapezoidal Decomposition I

There are 4 cases:

Motion Planning




Combinatorial Planning Algorithms: Trapezoidal Decomposition II

O(n lg n) running time. Easy to implement. Scales to higher dimensions.

Motion Planning




Combinatorial Planning Algorithms: Triangulation

O(n2) naive, O(n) optimal, O(n lg n) good tradeoff.

Motion Planning




Combinatorial Planning Algorithms: Maximum Clearance Roadmap

Based on deformation retract from topology.Imagine obtaining a skeleton by gradually thinning Cfree .Kind of a generalized Voronoi diagram.

Motion Planning




Combinatorial Planning Algorithms: Maximum Clearance Roadmap

Three cases:

O(n4), O(n lg n) optimal.Motion Planning




Combinatorial Planning Algorithms: Shortest Path Roadmap

Every reflex vertex (internal angle > ) is a roadmap vertex Edges in the roadmap correspond to two cases

Consecutive reflex vertices Bitanget edges

Motion Planning




Combinatorial Planning Algorithms: Higher Dimensions

If C is 3 or more dimensions, most methods do not extend.Optimal path planning for 3D polyhedra is NP-hard.Maximul clearance roadmaps become disconnected in 3D.Exception: Trapezoidal decomposition extends.

Motion Planning




Sampling-Based Planning

Explicit construction of the obstacle space is often intractable!

Use collision detector to separate planning from input geometry Systematically sample (random vs. deterministic) the configuration space Probe for freespace by querying a collision detection algorithm Single-query: Incremental sampling and searching Multiple-query: Precompute a roadmap, then search it for each query

Motion Planning




Sampling: Denseness

In topology, a set U is called dense in V if cl(U) = V .Implication: Every open subset of V contains at least one point in U.If U is dense and countable, a dense sequence can be formed:

: N U

Example: The rational numbers Q are dense in R.

Example: A random sequence is dense with probability one.

Motion Planning




Sampling: Quick refresher of topology

Let X be a topological space, and U be any subset of X.

If there exists an open set O1 so that x O1 and O1 U, then x is calledan interior point of U. The set of all such points is denoted int(U).

If there exists an open set O2 so that x O2 and O1 X \ U, then x iscalled an exterior point of U.

If x is neither an interior nor an exterior point, it is called a boundary point.The set of all those points is denoted U.

If x is either an interior or boundary point, its called a limit point of U.The set of all limit points of U is a closed set called the closure of U, and isdenoted by cl(U). Note: cl(U) = int(u) U.

Motion Planning




Sampling: Deterministic Alternative: The van der Corput sequence

Halton sequence: For each coordinate, use relatively prime bases. More uniformthan random (which needs O((lg n)1/d) as many samples to produce the sameexpected dispersion.Motion Planning




Sampling: Dispersion

Let P be a finite set of points in metric space (X,).The dispersion of P is:

(P) = supxX{minpP{(x , p)}}

In a bounded space, a dense sequence drives the dispersion to zero.

Motion Planning




Best possible Dispersion: Sukharev theorem

For any set P of k samples in [0, 1]d :

(P) 12bk 1d c

,

in which is the L dispersion.The best placement of k points:

Think: points per axis for any sample set. Holding the dispersion fixed requires exponentially many points indimension.Motion Planning




Sample-Based Planning: Framework

Given a single query: qI , qG Cfree.1. Initialization: Form G(V ,E ) with vertices qI , qG and no edges.2. Vertex Selection Method (VSM): Choose a vertex qcur V for

expansion.

3. Local Planning Method (LPM): For some qnew Cfree , attempt toconstruct a path s : [0, 1] Cfree so that (0) = qcur and (1) = qnew .

4. Insert an Edge in the Graph: Insert s into E, as an edge from qcur toqnew . If qnew is not already in V, it is inserted.

5. Check for a Solution:: Determine whether G encodes a solution path.6. Return to Step 2: Iterate unless a solution has beed found or the

algorithm reports failure.

Motion Planning




Sample-Based Planning: Random Tree vs. RRT

Rather than picking a vertex by chance, pick a configuration at random!

Motion Planning




Sample-Based Planning: RRT

Simpe Rapidly Exploring Random Tree:In each step, select a new configuration at random.Extend the nearest vertex (using the metric!) to this random point:

If there is an obstacle, then stop short:

If the closest point is an edge, its better to extend from there.

Motion Planning





Rapidly Exploring Random Trees (RRT) are one variant of Rapidly ExploringDense Trees (RDT), others may use a non-random dense sequence.

Motion Planning





Motion Planning




Sample-Based Planning: RRT: bi-directional search

Both trees are extended for each drawn random configuration.The trees are alternated, and the other currently second tree gets extendedusing the configuration that was added to the first.If connected, the solution is found.

Motion Planning





Grow RRT in the usual way When a new vertex xnew is added, try to connect to other RRT vertices

within radius .

Among all paths to the root from xnew , add a new RRT edge only for theshortest one.

If possible to reduce cost for other vertices within radius by connecting toxnew , then disconnect them from their parents and connect them throughxnew .

The radius is prescribed through careful percolation theory analysis(related to dispersion)

RRT yield asymptotically optimal paths through Cfree .

Motion Planning





Motion Planning




Sample-Based Planning: Bugtraps

Motion Planning




Sample-Based Planning: Probabilistic Roadmaps (PRM)

If multiple queries are expected for the same Cfree , building a roadmap may payoff.

Motion Planning




Sample-Based Planning: PRM - Variants

Connection Rules:

Nearest K: The K closest points to (i) are considered. Component K: Try to obtain up to K closest points of each connected

component of G. Radius: Take all points within a ball of radius r centered at (i). Visibility: Try connecting (i) to all vertices in G.

Motion Planning




Planning under Differential Constraints

Due to robot kinematics and dynamics, most systems are locally constrained inaddition to global obstacles.Let q represent the C-space velocity.In ordinary planning, any direction is allowed and the magnitude does notmatter.Thus we could say

q = u

and u Rn may be any velocity vector.

Motion Planning




Planning under Differential Constraints

More generally, a control system (or state transition equation) constrains thevelocity:

q = f (q, u)

and u belongs to some set U (usually bounded).A function u : T U is applied over a time interval T = [0, tf ] and theconfiguration q(t) at time t is given by

q(t) = q(0) +

t0

f (q(t ), u(t ))dt .

in which q(0) is the initial configuration.

Motion Planning




Planning under Differential Constraints: Dubins Car

This car drives only forward:

C = R2 S1.Let u = (us , u) and U = [0, 1] [max , max ].Control system of the form q = f (q, u) :

x = cos

y = sin

=usL

tan u

Motion Planning




Planning under Differential Constraints: Dubins Car

Stepping forward in the Dubins car:

Motion Planning




Planning under Differential Constraints: Boundary Value Problems

Motion Planning




Planning under Differential Constraints: RRT

For a RRT, just replace the straight line connection with a local planner:

Problems: need good metrics and primitives!

Motion Planning




Manipulation Planning

In most forms of motion planning, the robot is not allowed to touch objects!In manipulation planning, two phases are distinguished:

Transit mode: The robot moves towards a part. Transfer mode: The robot carries a part.

Transitions between these phases require specific grasping or stabilityrequirements, otherwise the part would fall out of the gripper when lifting ormove/topple after it is released.

Motion Planning




Planning in Mobile Manipulation - Our Take

Motion Planning




Planning in Mobile Manipulation - Our Take

We distinguish further:

Base Transit: The base moves to bring the object into reachability of thearms.

Pregrasp: The gripper is brought into a pose from where it can go straight. Grasping: The gripper goes straight to the grapsing pose and closes. Carry Pose: The object is moved into a specific place close to the robot to

facilitate navigation.

Base Transit: The base moves to bring the goal into reachability. Placing: The object is placed at the goal pose. Arm Retract: Similar to the Pregrasp, we move the gripper out in a

straight way and then park the arm.

Motion Planning




Practical Considerations: Self Filtering

Our sensors do not discriminate between the robot and the environment, so wehave to filter out parts of the robot from the sensor data before using it forcollision checking purposes.

Occluded obstacles may not be seen, so we need some persistancy in ourenvironment model!

Motion Planning




Practical Considerations: Representation

Octomaps are used to represent the environment, as they are they have a smallmemory footprint and allow for hierarchical queries.

Often, we want to encode free space, obstace space and unknown space.Therefore, 2 bits per node are needed.

Motion Planning




Practical Considerations: Attaching Object Models

As we use planning for manipulation, we want to be able to plan for trajectorieswhile holding objects.

Whenever an object is carried, its model gets attached to the robot model.

Motion Planning




Mobile Manipulation - Inverse Reachability

Where can i stand when i want to reach pose P with my gripper?

The precalculated inverse reachability map quickly answers this question.

Motion Planning




Mobile Manipulation - Inverse Reachability

Robot pose samples are drawn until a promising one is found.

Static collision: When placed there, would the robot, the arms collide withthe environment?

Arm planning: Can we find a trajectory via planning or can we execute thegiven trajectory without collision?

Note: We sacrifice completeness.

Motion Planning




Mobile Manipulation - 3D Navigation

The robot is sliced vertically and convex hulls for each slice are used to speed upcollision checks.We can then use RRT or any other planner as usual.Motion Planning




Exam Question

For an infinite sample sequence : N X , let k denote the first k samples.Find a metric space X Rn and so that:

1. The dispersion of k is for all k.2. The dispersion of is 0.

Hint: dont make it too complicated!

Motion Planning



Motion PlanningMotion Planning

Motion Planning

Documents

Transcript of Motion Planning