cs 4649/7649 robot intelligence: planning · s. joo (sungmoon.joo@cc.gatech.edu) 10/7/2014 1 cs...

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 1

CS 4649/7649Robot Intelligence: Planning

Sungmoon Joo

School of Interactive Computing

College of Computing

Georgia Institute of Technology

Differential Kinematics, Probabilistic Roadmaps

*Slides based in part on Dr. Mike Stilman’s lecture slides

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 2

Potential Fields

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 3

Potential Fields

*Simulation by Leng-Feng Lee@Buffalo Univ.

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 4

Summary: Navigation Algorithms

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 5

Coordinates

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 6

Homogeneous Transform 3D

(1) Change the representation of a vector in frame B to frame A

(2) Change a vector in a frame to another vector (rotated) in the

(3) Unit vectors of frame B represented(expressed) in frame A

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 7

Forward(Direct) Kinematics

x

y{0}

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 8

Forward Kinematics

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 9

Forward Kinematics

Representation of frame2 in frame0

Representation of frame3 in frame0

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 10

Denavit-Hartenberg (DH) Parameters

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 11

DH Parameters

link parameters

joint variable

For a revolute joint

*For a prismatic joint: di is a joint variable

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 12

DH Parameters

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 13

DH Parameters

*special links – first, last

*special cases – eg. parallel links

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 14

Kinematics

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 15

Inverse Kinematics

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 16

Configurations Space

Configuration SpaceWork Space

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 17

Configuration Space: IK Solution

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 18

Configuration Space: IK Solution

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 19

Configuration Space: IK Solution

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 20

Analytical Methods

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 21

Analytical Methods

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 22

Analytical IK

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 23

Infinitesimal Changes

Forward Kinematics

Differential

(Forward) Kinematics

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 24

Differential Kinematics

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 25

Differential Kinematics

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 26

How do we compute J? – Method1

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 27

How do we compute J? – Method1

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 28

How do we compute J? – Method1

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 29

How do we compute J? – Method2

Rigid body angular motion

Rigid body linear + angular motion

{A}

{B}

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 30

How do we compute J? – Method2

Consider the propagation of velocities: linear velocity & angular velocity

{i-1}

{i}

Linear velocity propagation

Angular velocity propagation

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 31

Linear velocity propagation

Angular velocity propagation

How do we compute J? – Method2

Prismatic Joint

Revolute Joint

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 32

How do we compute J? – Method2

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 33

How do we compute J? – Method2

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 34

How do we compute J? – Method2

1. Simulate a small displacement Δθi for each joint

2. Observe Δx and numerically compute J

• Less accurate, more time consuming

• Sometimes the only option

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 35

How do we compute J? – Method3

• Workspace goal

• How do we get a joint space goal?

• Assuming 6 D.O.F and J is full rank: Δq = J−1Δx

Iterate until convergence

• Otherwise Still Possible (Pseudo-Inverse & Variants): J+ = JT (JJT )−1

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 36

Gradient IK - How do we use J?

x1x2

xn

…q1q2

qn

…

• Generally applicable to n-DOF kinematics

• Many existing implementations

• Slower and unstable around singularities

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 37

Gradient IK

Goals for manipulators are really that complicated!

Add what it takes to represent obstacles:

But, does this remind you of anything?

Δq = J−1Δx

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 38

Planning

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 39

Planning – Manipulator + Potential Field

• First effective planning method for high-DOF robot arms

• Very fast (potential fields)

However:

• Local Minima

• Complex obstacle representation

• For Soundness must define obstacles with respect to each link

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 40

Planning – Manipulator + Potential Field

● Roadmap Methods:

– Visibility Graphs

No polygons in C-space. How would you get polygons?

– Voronoi Diagrams

Canny’s Thesis: NP-hard Algorithms for Motion Planning

● Cell Decomposition:

– Exact

Same troubles as Visibility Graphs.

– Approximate

Ok for 2-3 dimensions

Good luck in 4+ dimensions

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 41

Planning – Manipulator + Potential Field

Combine Kinematics & Geometric Planning

• Pre-1980

– Mainly 2D, 3D planners

– In case of kinematics:

Ignore robot geometry

Robot is an ethereal entity that obeys kinematics

• 1980s

– Potential Fields!

First feasible SOUND solution

Locally optimal control strategy

Not globally complete or optimal

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 42

Kinematic Planning Algorithms

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 43

Can we solve these planning problems?

http://www.kavrakilab.org/robotics/prm.html“Planning Algorithms”, S. Lavalle

• What did Visibility, Voronoi, Cells, Fields have in common?

- Some form of explicit environment representation

- Attempt at some form of optimality

• New concepts from 1990s:

- Forget optimality altogether

- Focus on Completeness

- Think about Free Space

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 44

Key Idea

• Previous roadmaps used features

related to actual obstacle features.

• Probabilistic Roadmaps (PRM)

- Features: Sampled free points

- Edges: Verified connections

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 45

A New Kind of Roadmap

• Lydia Kavraki ‘94, ‘96 – Present• Mark Overmars ’92, ‘96 - Present

“Probabilistic roadmaps for path planning in high-dimensional configuration spaces”

By Kavraki, Svestka, Latombe, and Overmars, 1996, IEEE Transactions on

Robotics and Automation

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 46

Probabilistic Roadmap: Step 1

Randomly sample a configuration: P Keep only if P is in Free Space

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 47

Probabilistic Roadmap: Step 1

Randomly sample a configuration: P Keep only if P is in Free Space

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 48

Probabilistic Roadmap: Step 2

For each node P find k nearest neighbors: Q1 … Qk

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 49

Probabilistic Roadmap: Step 3

For each node P find k nearest neighbors: Q1 … Qk

Use a local planner to test connectivity between P and Qi

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 50

Probabilistic Roadmap: Step 3

For each node P find k nearest neighbors: Q1 … Qk

Use a local planner to test connectivity between P and Qi

What could be a local planner? What happens next?

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 51

Probabilistic Roadmap: Step 4

For each node P find k nearest neighbors: Q1 … Qk

Use a local planner to test connectivity between P and Qi

Find a path: Uniform Cost, A*, …

10/7/2014S. Joo (sungmoon.joo@cc.gatech.edu) 52

We can solve these planning problem

http://www.kavrakilab.org/robotics/prm.html http://www.youtube.com/watch?v=FRGzsyXHBqQ