Motion PlanningMotion Planning

1. What is the motion planning problem?

2. What is the fundamental question in motion planning problem?

3. What is the basic problem formulation?

4. Configuration Space.

5. Formalism for Configuration Space.

6. What is a Path?

7. Extensions to the Basic Motion Planning Probem

8. Examples of Applications of Motion Planning

9. Examples of Practical Algorithms: Bug 2.

10.Examples : Probabilistic Roadmaps.

154 slides

What is the What is the motion planning motion planning

problem?problem?

Motion Planning for a robotic armMotion Planning for a robotic arm

Motion Planning for a mobile robotMotion Planning for a mobile robot

Motion planning in a real worldMotion planning in a real world

Motion Planning for a Motion Planning for a Humanoid Robot with Humanoid Robot with Many Degrees of Many Degrees of Freedom in real world Freedom in real world environmentenvironment

Motion Planning for unusual robots, reconfigurable robotsMotion Planning for unusual robots, reconfigurable robots

Motion Planning in Medical and Motion Planning in Medical and Chemical ApplicationsChemical Applications

Is It Easy?Is It Easy?

What is the What is the fundamental fundamental question in question in

motion planning?motion planning?

Goal of Motion PlanningGoal of Motion Planning

• Compute motion strategies, e.g.:– geometric paths – time-parameterized trajectories– sequence of sensor-based motion commands

• To achieve high-level goals, e.g.:– go to A without colliding with obstacles– assemble product P– build map of environment E– find object O

Fundamental QuestionFundamental QuestionAre two given points connected by a path?

Valid region

Forbidden region

Fundamental QuestionFundamental QuestionAre two given points connected by a path?

Valid region

Forbidden region

E.g.:▪Collision with obstacle▪Lack of visibility of an object▪Lack of stability

What is the What is the formulation of the formulation of the basic problem of basic problem of

path finding path finding ??

Basic Problem of Basic Problem of Path Path planningplanning

Statement: Compute a collision-free path for a rigid or articulated object (the robot) among static obstacles

Inputs:– Geometry of robot and obstacles– Kinematics of robot (degrees of freedom)– Initial and goal robot configurations (placements)

Output:– Continuous sequence of collision-free robot

configurations connecting the initial and goal configurations

Examples with Rigid ObjectExamples with Rigid Object

Ladder problem

Piano-mover problem

Example with Articulated Example with Articulated ObjectObject

Configuration Configuration SpaceSpace

Tool: Configuration SpaceTool: Configuration Space

Compare!Compare!

Valid region

Forbidden region

Tool: Configuration SpaceTool: Configuration Space

Problems:• Geometric complexity• Space dimensionality

FormalismFormalism for for Configuration Configuration

SpaceSpace

Notation for Configuration Notation for Configuration SpaceSpace

Point Robot in Configuration space

Point robot in Three-Dimensional Configuration space

Translation and rotation of a robot

Example of simple configuration space for a robot with 2DOF in planar space

Representation of points in configuration space

In presence of obstacles, where the robot can actually move?

Free SpaceFree Space

Think of R as a robot and P as an obstacle

Robot that can translate and rotate in 2D space gives another dimension to the problem

C-obstacle in Three-Dimensional C-obstacle in Three-Dimensional SpaceSpace

This is twisted

Can we stay in 2D? x

x

But is it good?

Too conservativeToo conservative

What is a What is a Path?Path?

What is a Path?What is a Path?

Path is a very general concept considered from the point of view of computational geometry, time and search

Tool: Configuration SpaceTool: Configuration Space(C-Space C)(C-Space C)

q=(q1,…,qn)

Configuration SpaceConfiguration Space

qq11

qq22

qq33

qqnn

In real problems configuration space is highly dimensional

Definition of Robot Definition of Robot ConfigurationConfiguration

A robot configurationconfiguration is a specification of the positions of all robot points relative to a fixedfixed coordinate system

Usually a configuration is expressed as a “vector” “vector” of position/orientation parameters

reference point

RigidRigid Robot Example Robot Example

• 3-parameter representation: q = (x,y,)• In a 3-D workspace q would be of the

form (x,y,z,)

x

y

robotreference direction

workspace

Like you need six-dimensional space to describe the airplane in the air

ArticulatedArticulated Robot Robot ExampleExample

qq11

qq22

q = (q1,q2,…,q10)

Protein exampleProtein example

What is the What is the Configuration Space of Configuration Space of

a Robot?a Robot?Space of all its possible configurationsBut the topologytopology of this space is usually not that of a CartesianCartesian spaceC = S1 x S1

Configuration Space of a Configuration Space of a RobotRobot

Space of all its possible configurationsBut the topology of this space is usually not that of a Cartesian space

C = S1 x S1

Configuration Space of a Configuration Space of a RobotRobot

Space of all its possible configurationsBut the topology of this space is usually not that of a Cartesian space

C = S1 x S1

But here looks like a cartesian space

Like a torus

What is a Topology of the What is a Topology of the Configuration Space?Configuration Space?

qq11

qq22

(S1)7xR3

R

R

R

S1

S1 is rotationR is linear motion

Structure of Configuration Structure of Configuration SpaceSpace

Structure of Configuration Space is a manifoldFor each point q, there is a 1-to-1 map between a neighborhood of q and a Cartesian space Rn, where n is the dimension of C

This map is a local coordinate system called a chart.

C can always be covered by a finite number of charts. Such a set is called an atlas

ExampleExampleEvery point of a sphere but one can be mapped to a plane

reference point

Case of a Case of a PlanarPlanar RigidRigid RobotRobot

• 3-parameter representation: q = (x,y,) with [0,2). Two charts are needed

• Other representation: q = (x,y,cos,sin)c-space is a 3-D cylinder R2 x S1 embedded in a 4-D space

x

y

robotreference direction

workspace

Rigid Robot in 3-D Rigid Robot in 3-D WorkspaceWorkspace

• q = (x,y,z,)

• Other representation: q = (x,y,z,r11,r12,…,r33) where r11, r12, …, r33 are the elements of rotation matrix R: r11 r12 r13

r21 r22 r23 r31 r32 r33

with: – ri1

2+ri22+ri3

2 = 1– ri1rj1 + ri2r2j + ri3rj3 = 0– det(R) = +1

The c-space is a 6-D space (manifold) embedded in a 12-D Cartesian space. It is denoted by R3xSO(3)

MetricMetric in Configuration in Configuration SpaceSpace

A metric or distance function d in C is a mapmap

d: (q1,q2) C2 d(q1,q2) > 0such that:

– d(q1,q2) = 0 if and only if q1 = q2

– d(q1,q2) = d (q2,q1)

– d(q1,q2) < d(q1,q3) + d(q3,q2)

The general concept of metric or “distance” in some kind of space

Metric in Configuration Metric in Configuration SpaceSpace

Example:• Robot A and point x of A

– x(q): location of x in the workspace when A is at configuration q

– A distance d in C is defined by:d(q,q’) = maxxA ||x(q)-x(q’)||

where ||a - b|| denotes the Euclidean distance between points a and b in the workspace

Specific Examples in Specific Examples in RR22 x x SS11

q = (x,y,), q’ = (x’,y’,’) with ’ [0,2)= min{|’| , 2|’|}

d(q,q’) = sqrt[(x-x’)2 + (y-y’)2 + 2]d(q,q’) = sqrt[(x-x’)2 + (y-y’)2 + ()2]where is the maximal distance between the reference point and a robot point

’’

What is a What is a PathPath??

A path in C is a piece of continuous curve connecting two configurations q and q’:

: s [0,1] (s) C s’ s d((s),(s’)) 0

q1

q3

q0

qn

q4

q2

(s)

Other Possible Constraints on Other Possible Constraints on PathPath

Finite length, smoothness, curvature, etc… A trajectory is a path parameterized by

time: : t [0,T] (t) C

q1

q3

q0

qn

q4

q2

(s)

Obstacles Obstacles in C-Spacein C-Space

Obstacles in C-SpaceObstacles in C-Space

A configuration qq is collision-free, or free, if the robot placed at qq has null intersection null intersection with the obstacles in the workspace

The free space F is the set of free configurations

A C-obstacle is the set of configurations where the robot collides robot collides with a given workspace obstacle

A configuration is semi-free if the robot at this configuration touches obstacles without overlap without overlap

DiscDisc Robot in 2-D Robot in 2-D WorkspaceWorkspace

Touches obstacle without overlap

BCB

Rigid Robot Translating in Rigid Robot Translating in 2-D2-D

CB = B A = {b-a | aA, bB}

a1

b1

b1-a1

Rigid Robot Rigid Robot Translating in Translating in 2-D2-D

CB = B A = {b-a | aA, bB}

a1

b1

b1-a1

Linear-Time Computation Linear-Time Computation of of

C-Obstacle in 2-DC-Obstacle in 2-D(convex polygons)

O(n+m)

Rigid Robot Rigid Robot TranslatingTranslating andand RotatingRotating in 2-D in 2-D

=/2

=0

Now configuration space has 3 dimensions

C-Obstacle for C-Obstacle for ArticulatedArticulated RobotRobot

Two degrees of freedom

Extensions to Extensions to Basic Motion Basic Motion

Planning Planning ProblemProblem

Some Extensions of Basic Some Extensions of Basic ProblemProblem

1. Moving obstacles2. Multiple robots3. Movable objects4. Assembly planning5. Goal is to acquire

information by sensing1. Model building2. Object finding/tracking3. Inspection

6. Nonholonomic constraints

7. Dynamic constraints8. Stability constraints

1. Optimal planning2. Uncertainty in model,

control and sensing3. Exploiting task

mechanics (sensorless motions, under-actualted systems)

4. Physical models and deformable objects

5. Integration of planning and control

6. Integration with higher-level planning

Aerospace Robotics Lab Aerospace Robotics Lab RobotRobot

air bearing

gas tank

air thrusters

obstacles

robot

Total duration : 40 sec

Two concurrent planning goals:• Reach the goal• Reach a safe region

Autonomous HelicopterAutonomous Helicopter

[Feron] (MIT)

Assembly PlanningAssembly Planning

Map BuildingMap Building

Where to move next?

Target FindingTarget Finding

The guard guards the building attacked by terrorists. What he sees is colored yellow

Target TrackingTarget Tracking

Planning for Nonholonomic Planning for Nonholonomic RobotsRobots

Under-Actuated SystemsUnder-Actuated Systems

video

[Lynch] (Northwestern)

Planning with Uncertainty in Planning with Uncertainty in Sensing and ControlSensing and Control

I

GWW11

WW22

Planning with Uncertainty in Planning with Uncertainty in Sensing and ControlSensing and Control

I

GWW11

WW22

Planning with Uncertainty in Planning with Uncertainty in Sensing and ControlSensing and Control

I

GWW11

WW22

Motion Planning for Motion Planning for Deformable ObjectsDeformable Objects

[Kavraki] (Rice)

Examples of Examples of Applications of Applications of

Motion Motion PlanningPlanning

Examples of ApplicationsExamples of Applications

1. Manufacturing:1. Robot programming2. Robot placement3. Design of part feeders

2. Design for manufacturing and servicing

3. Design of pipe layouts and cable harnesses

4. Autonomous mobile robots planetary exploration, surveillance, military scouting

1. Graphic animation of “digital actors” for video games, movies, and webpages

2. Virtual walkthru3. Medical surgery

planning4. Generation of

plausible molecule motions, e.g., docking and folding motions

5. Building code verification

Robot ProgrammingRobot Programming

Robot PlacementRobot Placement

Design for Design for Manufacturing/ServicingManufacturing/Servicing

General ElectricGeneral Electric

General MotorsGeneral MotorsGeneral MotorsGeneral Motors

Assembly Planning and Design Assembly Planning and Design of Manufacturing Systemsof Manufacturing Systems

Part FeedingPart Feeding

Part FeedingPart Feeding

Cable Harness/ Pipe designCable Harness/ Pipe design

Humanoid RobotHumanoid Robot

[Kuffner and Inoue, 2000] (U. Tokyo)

Modular Reconfigurable Modular Reconfigurable RobotsRobots

Xerox, ParcXerox, Parc

Casal and Yim, 1999

Military Scouting and Military Scouting and Planet ExplorationPlanet Exploration

[CMU, NASA]

Digital ActorsDigital Actors

A Bug’s Life (Pixar/Disney) Toy Story (Pixar/Disney)

Tomb Raider 3 (Eidos Interactive) Final Fantasy VIII (SquareOne)The Legend of Zelda (Nintendo)

Antz (Dreamworks)

Motion Planning for Digital Motion Planning for Digital ActorsActors

Manipulation

Sensory-based locomotion

Navigation Through Virtual Navigation Through Virtual EnvironmentsEnvironments

[Cheng-Chin U., UNC, Utrecht U.]

video

Building Code VerificationBuilding Code Verification

Radiosurgical PlanningRadiosurgical Planning

Cross-firing at a tumor while sparing healthy

critical tissue

Study of Study of the Motion of Bio-Moleculesthe Motion of Bio-Molecules

• Protein folding• Ligand binding

Practical Applications in Practical Applications in ProjectsProjects

Present a coherent framework for motion planning problems

Emphasis of “practical” algorithms with some guarantees of performance over “theoretical” or purely “heuristic” algorithms

FrameworkFramework

Continuous representation(configuration space and related spaces + constraints)

Discretization(random sampling, criticality-based decomposition)

Graph searching(blind, best-first, A*)

Practical Algorithms (1/2)Practical Algorithms (1/2)

A complete motion planner always returns a solution plan when one exists and indicates that no such plan exists otherwise.

Most motion planning problems are hard, meaning that complete planners take exponential time in # of degrees of freedom, objects, etc.

Practical Algorithms (2/2)Practical Algorithms (2/2)

Theoretical algorithms strive for completeness and minimal worst-case complexity. Difficult to implement and not robust.Heuristic algorithms strive for efficiency in commonly encountered situations. Usually no performance guarantee. Weaker completeness Simplifying assumptions Exponential algorithms that work in practice

Programming ProjectProgramming Project• Navigate in virtual environment

• Simulate legged robot

• Inspection of structures

• Search and escape

SourcesSources

Davi GeigerDavi Geiger

Jean-Claude LatombeJean-Claude Latombe