Introduction to Robot Introduction to Robot Motion PlanningMotion Planning

Robotics meet Computer Robotics meet Computer ScienceScienceRobotics meet Computer Robotics meet Computer ScienceScience

ExampleExampleA robot arm is to build an assembly from a A robot arm is to build an assembly from a

set of parts.set of parts.

Tasks for the robot:Tasks for the robot:

• Grasping: position gripper on objectGrasping: position gripper on object

design a path to this positiondesign a path to this position

• Transferring: determine geometry path for Transferring: determine geometry path for armarm

avoid obstacles + clearanceavoid obstacles + clearance

• PositioningPositioning

Information requiredInformation required• Knowledge of spatial arrangement of Knowledge of spatial arrangement of

wkspace. E.g., location of obstacleswkspace. E.g., location of obstacles

• Full knowledge Full knowledge full motion full motion planningplanning

• Partial knowledge Partial knowledge combine combine planning and executionplanning and execution

motion planning = collection of motion planning = collection of problemsproblems

Basic ProblemBasic Problem

A simplified version of the problem A simplified version of the problem assumesassumes

• Robot is the only moving object in the Robot is the only moving object in the wkspacewkspace

• No dynamics, no temporal issuesNo dynamics, no temporal issues

• Only non-contact motionsOnly non-contact motions

MP = pure “geometrical” problemMP = pure “geometrical” problem

Components of BMPPComponents of BMPP• AA: single rigid object - the robot - moving in : single rigid object - the robot - moving in

Euclidean space Euclidean space W W (the wkspace). (the wkspace).

W = RW = RNN, N=2,3, N=2,3

• BBii , I=1,…,q. Rigid objects in , I=1,…,q. Rigid objects in WW. The obstacles. The obstacles

AABB22

BB11

Components of BMPP Components of BMPP (cont.)(cont.)AssumeAssume

• Geometry of Geometry of AA and and BBii is perfectly is perfectly knownknown

• Location of Location of BBii is known is known

• No kinematic constraints on No kinematic constraints on AA: a : a “free flying” object“free flying” object

Components of BMPP Components of BMPP (cont.)(cont.)

• The Problem:The Problem:

– Given an initial position and orientation PO init

– Given a goal position and orientation POgoal

– Generate: continuous path t from POinit to POgoal

• t is a continuous sequence of Pos’

Configuration Space IdeaConfiguration Space Idea1. represent robot as point in space

2. map obstacles into this space

3. transform problem from planning object motion to planning point motion

AABB22

BB11

Configuration Space Configuration Space (cont.)(cont.)

W: Euclidean space in which motion occurs

A at a given position is a compact in W. Attach FA

Bi closed subset of W .

FFw w is a frame fixed in W

AABB22

BB11

FW

FA

Notion of CS (cont)Notion of CS (cont)Def: configuration of an object

Position of every point of the object w.r.t. FW

Def: Configuration q of A

Postion T and orientation O of FA w.r.t. FW

Def: configuration space of A

Is the space T T of all configurations of A

• A(q) : subset of W occupied by A at q

• a(q) : is a point in A(q)

Information RequiredInformation Required• Example: T: N-dimensional vector

O: NxN rotation matrix

• In this case, q = (T,O), a subset of RN(N+1)

• Note that C is locally like R3 or R6.

Notice: no global correspondence

Mathematic Notion of PathMathematic Notion of Path• Need a notion of continuity

• Define a distance function d : C x C -> R+

– Example: d(q,q’) = maxa in A ||a(q) - a(q’)||

y

x

q

'

'

'

'

y

x

qd

Notion of Path (cont.)Notion of Path (cont.)

• Def: A path of A from qinit to qgoal

Is a continuous map t : [0,1] -> C

s.t. t(0) = qinit and t (1) = qgoal

• Property: t is continuous if for each so in (0,1), lim d(s,so) = 0 when s -> so

• Obstacle Bi maps in C to a region

CBi = {q in C, s.t. A(q) and Bi are not disjoint

• Example: “round” robot with no preferred orientation

Obstacles in Configuration Obstacles in Configuration SpaceSpace

B B

CB

Original Space Configuration Space

Obstacles in C- Space Obstacles in C- Space (cont.)(cont.)• Obstacles in C are called C-obstacles.

• C-obstacle region: Union of all Cbi

• Free space: Cfree = C - U Cbi

• q is a free configuration if q belongs to Cfree

• Def: Free Path.

Is a path between qinit and qgoal , t: [0,1] -> Cfree

Obstacles in Obstacles in CC (cont.) (cont.)• Def: Connected Component

q1, q2 belong to the same connected component of Cfree iff they are connected by a free path

Objective of Motion Planning: Objective of Motion Planning: generate a free path between 2 generate a free path between 2 configurations if one exists configurations if one exists oror report that report that no free path exists.no free path exists.

Examples of Examples of C-ObstaclesC-Obstacles• Translational Case:

1. A is a single point -> no orientation

W = RN = C

2. A is a disk or dimensioned object allow to translate freely but without rotation.

C-Obstacles: obstacles “grown” by the shape of A

Planning ApproachesPlanning Approaches• 3 approaches: road maps, cell

decomposition and potential field

1- Roadmap

Captures connectivity of Cfree in a network of 1-D curves called “the roadmaps.”

Once a roadmap is constructed: use a standard path.

Roadmap Construction Methods: 1) Visibility Graph, 2) Voronoi Diagram, 3) Freeway Net and 4) Silhouette.

Roadmaps ExamplesRoadmaps Examples

qinit

qgoal

Visibility graph in 2D CS. Nodes: initial and final config + vertices of C-obstacles.

Voronoi diagram with polygonal C-obstacles

qinit

qgoal

Cell DecompositionCell Decomposition• Decompose the free space into simple Decompose the free space into simple

regions called cellsregions called cells

• Construct a non-directed graph Construct a non-directed graph representing adjacencies: the representing adjacencies: the conectivity conectivity graphgraph

• Search for a path forming a “channel”Search for a path forming a “channel”

• Two variations:Two variations:– Exact: union of cells is exactly the free spaceExact: union of cells is exactly the free space

– Approximate: union included in the free Approximate: union included in the free spacespace

Cell Decomposition: Cell Decomposition: ExampleExample

10

1

2

3 4 5

6 7

8

911 12

3

2 10

4 56

9

1

11 12

7

8

qinit

qgoal

Extensions of the Basic Extensions of the Basic ProblemProblem

• Multiple moving objectsMultiple moving objects– Multiple obstaclesMultiple obstacles

– Multiple RobotsMultiple Robots

– Articulated RobotsArticulated Robots

• Kinematic ConstraintsKinematic Constraints

• UncertaintyUncertainty

• Movable objectsMovable objects

Computational ComplexityComputational Complexity• Instances may differ in “size”: dimension Instances may differ in “size”: dimension

of C-space and # of obstaclesof C-space and # of obstacles

• Result 1Result 1: : planning a free path for a robot made of an arbitrary # of polyhedral bodies connected by joints, among a finite set of polyhedral obstacles is a PSPACE-hard problem

• Result 2Result 2: : A free path in a C-space of fixed dimension m, when the free space is defined by n polynomials of max degree d, can be computed exponentially in m and polynomial in n and d