Motion Planning Motion 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 Examples : Probabilistic Roadmaps. 154 slides

What is the motion planning problem?

Motion Planning for a robotic arm

Motion Planning for a mobile robot

Motion planning in a real world

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

Motion Planning for unusual robots, reconfigurable robots

Motion Planning in Medical and Chemical Applications

Is It Easy?

What is the fundamental question in motion planning?

Goal 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 Question Are two given points connected by a path? Valid region Forbidden region

Fundamental Question Are 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 formulation of the basic problem of path finding ?

Basic Problem of Path planning 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 Object  Ladder problem Piano-mover problem 

Example with Articulated Object

Configuration Space

Tool: Configuration Space

Compare! Valid region Forbidden region

Tool: Configuration Space Problems: Geometric complexity Space dimensionality

Formalism for Configuration Space

Notation for Configuration Space

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 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 Space This is twisted

Can we stay in 2D? x

x

But is it good?

Too conservative

What is a Path?

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

Tool: Configuration Space (C-Space C)

q=(q 1,…,q n ) Configuration Space q1q1q1q1 q2q2q2q2 q3q3q3q3 qnqnqnqn In real problems configuration space is highly dimensional

Definition of Robot Configuration configuration fixed A robot configuration is a specification of the positions of all robot points relative to a fixed coordinate system “vector” Usually a configuration is expressed as a “vector” of position/orientation parameters

reference point Rigid Robot Example 3-parameter representation: q = (x,y,  ) In a 3-D workspace q would be of the form (x,y,z,  ) x y  robot reference direction workspace Like you need six-dimensional space to describe the airplane in the air

Articulated Robot Example q1q1q1q1 q2q2q2q2 q = (q 1,q 2,…,q 10 )

Protein example

What is the Configuration Space of a Robot? Space of all its possible configurations topology Cartesian But the topology of this space is usually not that of a Cartesian space C = S 1 x S 1

Configuration Space of a Robot Space of all its possible configurations But the topology of this space is usually not that of a Cartesian space C = S 1 x S 1

Configuration Space of a Robot Space of all its possible configurations But the topology of this space is usually not that of a Cartesian space C = S 1 x S 1 But here looks like a cartesian space Like a torus

What is a Topology of the Configuration Space? q1q1q1q1 q2q2q2q2 (S1) 7 xR 3 R R R S1 S1 is rotation R is linear motion

Structure of Configuration Space Structure of Configuration Space is a manifold For each point q, there is a 1-to-1 map between a neighborhood of q and a Cartesian space R n, 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

Example Every point of a sphere but one can be mapped to a plane

reference point Case of a Planar Rigid Robot 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 R 2 x S 1 embedded in a 4-D space x y  robot reference direction workspace

Rigid Robot in 3-D Workspace q = (x,y,z,  ) Other representation: q = (x,y,z,r 11,r 12,…,r 33 ) where r 11, r 12, …, r 33 are the elements of rotation matrix R: r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 with: –r i1 2 +r i2 2 +r i3 2 = 1 –r i1 r j1 + r i2 r 2j + r i3 r j3 = 0 –det(R) = +1 The c-space is a 6-D space (manifold) embedded in a 12-D Cartesian space. It is denoted by R 3 xSO(3)

Parameterization of SO(3) EulerEuler angles: (  quaternionUnit quaternion: (cos  /2, n 1 sin  /2, n 2 sin  /2, n 3 sin  /2) x yz x yz x y z  x yz 1  2  3  4

Metric in Configuration Space map A metric or distance function d in C is a map d: (q 1,q 2 )  C 2  d(q 1,q 2 ) > 0 such that: –d(q 1,q 2 ) = 0 if and only if q 1 = q 2 –d(q 1,q 2 ) = d (q 2,q 1 ) –d(q 1,q 2 ) < d(q 1,q 3 ) + d(q 3,q 2 ) The general concept of metric or “distance” in some kind of space

Metric in Configuration Space 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’) = max x  A ||x(q)-x(q’)|| where ||a - b|| denotes the Euclidean distance between points a and b in the workspace

Specific Examples in R 2 x S 1 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 Path?  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 q 1 q 3 q 0 q n q 4 q 2  (s)

Other Possible Constraints on Path  Finite length, smoothness, curvature, etc…  A trajectory is a path parameterized by time:  : t  [0,T]   (t)  C q 1 q 3 q 0 q n q 4 q 2  (s)

Obstacles in C-Space

q qnull intersection A configuration q is collision-free, or free, if the robot placed at q has null intersection with the obstacles in the workspace The free space F is the set of free configurations robot collides A C-obstacle is the set of configurations where the robot collides with a given workspace obstacle without overlap A configuration is semi-free if the robot at this configuration touches obstacles without overlap

Disc Robot in 2-D Workspace Touches obstacle without overlap B CB

Rigid Robot Translating in 2-D CB = B A = {b-a | a  A, b  B} a1 b1 b1-a1

Rigid Robot Translating in 2-D CB = B A = {b-a | a  A, b  B} a1 b1 b1-a1

Linear-Time Computation of C-Obstacle in 2-D (convex polygons) O(n+m)

Rigid Robot Translating and Rotating in 2-D   =  /2  =0 Now configuration space has 3 dimensions

C-Obstacle for Articulated Robot Two degrees of freedom

DEFINITIONS OF : Free and Semi-Free Paths  A free path lies entirely in the free space F  A semi-free path lies entirely in the semi-free space

Remark on Free-Space Topology 1.The robot and the obstacles are modeled as closed subsets, meaning that they contain their boundaries 2.One can show that the C-obstacles are closed subsets of the configuration space C as well 3.Consequently, the free space F is an open subset of C. 1.Hence, each free configuration is the center of a ball of non- zero radius entirely contained in F 4.The semi-free space is a closed subset of C. 1.Its boundary is a superset of the boundary of F

The semi-free space is a closed subset of C. Its boundary is a superset of the boundary of F obstacle Free space semi-free space

Notion of Homotopic Paths Two paths with the same endpoints are homotopic if one can be continuously deformed into the other R x S 1 example:  1 and  2 are homotopic  1 and  3 are not homotopic In this example, infinity of homotopy classes q q’   

Connectedness of C-Space 1.C is connected if every two configurations can be connected by a path 2.C is simply-connected if any two paths connecting the same endpoints are homotopic Examples: R 2 or R 3 3.Otherwise C is multiply-connected Examples: S 1 and SO(3) are multiply- connected: - In S 1, infinity of homotopy classes - In SO(3), only two homotopy classes

Extensions to Basic Motion Planning Problem

Some Extensions of Basic Problem 1.Moving obstacles 2.Multiple robots 3.Movable objects 4.Assembly planning 5.Goal is to acquire information by sensing 1.Model building 2.Object finding/tracking 3.Inspection 6.Nonholonomic constraints 7.Dynamic constraints 8.Stability constraints 1.Optimal planning 2.Uncertainty in model, control and sensing 3.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 Robot air bearing gas tank air thrusters obstacles robot

Total duration : 40 sec Two concurrent planning goals: Reach the goal Reach a safe region

Autonomous Helicopter [Feron] (MIT)

Assembly Planning

Map Building Where to move next?

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

Target Tracking

Planning for Nonholonomic Robots

Under-Actuated Systems video [Lynch] (Northwestern)

Planning with Uncertainty in Sensing and Control I G W1W1W1W1 W2W2W2W2

I G W1W1W1W1 W2W2W2W2

I G W1W1W1W1 W2W2W2W2

Motion Planning for Deformable Objects [Kavraki] (Rice)

Examples of Applications of Motion Planning

Examples of Applications 1.Manufacturing: 1.Robot programming 2.Robot placement 3.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 walkthru 3.Medical surgery planning 4.Generation of plausible molecule motions, e.g., docking and folding motions 5.Building code verification

Robot Programming

Robot Placement

Design for Manufacturing/Servicing General Electric General Motors

Assembly Planning and Design of Manufacturing Systems

Part Feeding

Cable Harness/ Pipe design

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

Modular Reconfigurable Robots Xerox, Parc Casal and Yim, 1999

Military Scouting and Planet Exploration [CMU, NASA]

Digital 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 Actors Manipulation Sensory-based locomotion

Navigation Through Virtual Environments [Cheng-Chin U., UNC, Utrecht U.] video

Building Code Verification

Radiosurgical Planning Cross-firing at a tumor while sparing healthy critical tissue

Study of the Motion of Bio-Molecules Protein folding Ligand binding

Practical Applications in Projects  Present a coherent framework for motion planning problems  Emphasis of “practical” algorithms with some guarantees of performance over “theoretical” or purely “heuristic” algorithms

Framework Continuous representation (configuration space and related spaces + constraints) Discretization (random sampling, criticality-based decomposition) Graph searching (blind, best-first, A*)

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) 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 Project Navigate in virtual environment Simulate legged robot Inspection of structures Search and escape

Sources Davi Geiger Jean-Claude Latombe