Real-time motion planning for

Manipulator based on Configuration Space

Chen KemingCis Peking University

Main Contents

Introduction My current work Future work and related work C-Space visualization for Teleoperation

Introduction Manipulator Motion Planning Problems

– Statement:• Compute a collision-free path for a manipulator among

obstacles

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

– Outputs:• Continuous sequence of collision-free manipulator

configurations connecting the initial and goal configurations

Introduction

Tool: Configuration Space

Introduction

FrameworkManipulator

representation

Obstacles representation

Configuration space formulation

DiscretizationGraph

searching

My Current Work Motivation: Towards real-time

Human-Robot Interaction in dynamic environment

Application– (Mobile based) Manipulator interacts

with human without collision– Dual-arm robot (Chen Fen,Ding Fu-qiang

and Zhao Xi-fang “Collision-free Path Planning of dual-arm Robot.” ROBOT,vol.24,Mar.2002)

My Current Work Assumption

– The input data are readily available at any time

Manipulator representation– Cylinders– Reduction to 3 joints

Obstacles representation– Cylinders– Combination of main body and arms

My Current Work

C-Space formulation– Reduction to determine whether 2 cylinders

collide in 3D W-Space

Case 1: Case 2:

My Current Work

– Schematic

My Current Work

– Goal configurations formulation using inverse kinematics

Discretization– Joint 1: 161, Joint 2: 71, Joint 3: 121

My Current Work

Lazy C-Space computation due to– Large numbers of points in C-Space(total

1,383,151 points)– Real-time process requirement

Graph searching (A*)– Why use A*

• Optimal and complete

• Objective values (expanding nodes, time)

My Current Work

– Speed up A*• OPEN is implemented as

– hash table

– priority list(implemented as Binary Heap)

• CLOSED is implemented as hash table

List implementation Hash table and Binary Heap implementation

An example (collision checking points: more than 30000)

My Current Work

Result:

My Current Work Dealing with dynamic environment

– A* Replanner: Plan by A* using all the available information at the start.

– Start tracing the optimal path– If there is a discrepancy between the initial ma

p and the actual environment, update the new cost values for the corresponding arcs, run A* again for planning between the current position and the goal.

My Current Work

A* Replanner: shortcoming– If the goal configuration is far away, little cha

nges may force the planner to use A* over the whole C-Space, although the changes in the optimal path may be small

– Hence, A* replanner can be grossly inefficient computationally for real-time process

My Current Work Optimization --- Dynamic A*(D*) [Stentz, 1994]

– Functionally equivalent to A* replanner– Make “local” changes to the map and the resultant opt

imal path when a discrepancy between map and the environment is found

• Essentially prunes the graph search

– So, D* could be a proper choice for optimization. But so far, it has only been used in mobile robotics to move a robot to given goal coordinates in unknown terrain [Koenig, 2002].

D* Algorithm

c(x1,x2)=1c(x1,x3)=1.4

c(x1,x8)=10000,if x8 is in obstacle,x1 is a freecell

c(x1,x9)=10000.4, if x9 is in obstacle, x1 is a freecell

x9 x2 x3

x8 x1 x5

x7 x6 x7

6

h=6

k=6

b=

h=5

k=5

b=

h=4

k=4

b=

h=3

k=3

b=

h=2

k=2

b=

h=1

k=1

b=

h=0

k=0

b=

5

h=6.4

k=6.4

b=

h=5.4

k=5.4

b=

h=4.4

k=4.4

b=

h=3.4

k=3.4

b=

h=2.4

k=2.4

b=

h=1.4

k=1.4

b=

h=1

k=1

b=

4

h=6.8

k=6.8

b=

h=5.8

k=5.8

b=

h=4.8

k=4.8

b=

h=3.8

k=3.8

b=

h=2.8

k=2.8

b=

h=2.4

k=2.4

b=

h=2

k=2

b=

3

h=7.2

k=7.2

b=

h=6.2

k=6.2

b=

h=5.2

k=5.2

b=

h=4.2

k=4.2

b=

h=3.8

k=3.8

b=

h=3.4

k=3.4

b=

h=3

k=3

b=

2

h=7.6

k=7.6

b=

h=6.6

k=6.6

b=

h=5.6

k=5.6

b=

h=5.2

k=5.2

b=

h=4.8

k=4.8

b=

h=4.4

k=4.4

b=

h=4

k=4

b=

1

h=8.0

k=8.0

b=

h=7.0

k=7.0

b=

h=6.6

k=6.6

b=

h=6.2

k=6.2

b=

h=5.8

k=5.8

b=

h=5.4

k=5.4

b=

h=5

k=5

B=

r/c 1 2 3 4 5 6 7

Goal

Start

Gate

6

h=6

k=6

b=

h=5

k=5

b=

h=4

k=4

b=

h=3

k=3

b=

h=2

k=2

b=

h=1

k=1

b=

h=0

k=0

b=

5

h=6.4

k=6.4

b=

h=5.4

k=5.4

b=

h=4.4

k=4.4

b=

h=3.4

k=3.4

b=

h=2.4

k=2.4

b=

h=1.4

k=1.4

b=

h=1

k=1

b=

4

h=6.8

k=6.8

b=

h=5.8

k=5.8

b=

h=4.8

k=4.8

b=

h=3.8

k=3.8

b=

h=2.8

k=2.8

b=

h=2.4

k=2.4

b=

h=2

k=2

b=

3

h=7.2

k=7.2

b=

h=6.2

k=6.2

b=

h=5.2

k=5.2

b=

h=4.2

k=4.2

b=

h=3.8

k=3.8

b=

h=3.4

k=3.4

b=

h=3

k=3

b=

2

h=7.6

k=7.6

b=

h=6.6

k=6.6

b=

h=5.6

k=5.6

b=

h=5.2

k=5.2

b=

h=4.8

k=4.8

b=

h=4.4

k=4.4

b=

h=4

k=4

b=

1

h=8.0

k=8.0

b=

h=7.0

k=7.0

b=

h=6.6

k=6.6

b=

h=6.2

k=6.2

b=

h=5.8

k=5.8

b=

h=5.4

k=5.4

b=

h=5

k=5

b=

r/c 1 2 3 4 5 6 7

(7,6) 0

State k

6

h=6

k=6

b=

h=5

k=5

b=

h=4

k=4

b=

h=3

k=3

b=

h=2

k=2

b=

h=1

k=1

b=(7,6)

h=0

k=0

b=

5

h=6.4

k=6.4

b=

h=5.4

k=5.4

b=

h=4.4

k=4.4

b=

h=3.4

k=3.4

b=

h=2.4

k=2.4

b=

h=1.4

k=1.4

b=(7,6)

h=1

k=1

b=(7,6)

4

h=6.8

k=6.8

b=

h=5.8

k=5.8

b=

h=4.8

k=4.8

b=

h=3.8

k=3.8

b=

h=2.8

k=2.8

b=

h=2.4

k=2.4

b=

h=2

k=2

b=

3

h=7.2

k=7.2

b=

h=6.2

k=6.2

b=

h=5.2

k=5.2

b=

h=4.2

k=4.2

b=

h=3.8

k=3.8

b=

h=3.4

k=3.4

b=

h=3

k=3

b=

2

h=7.6

k=7.6

b=

h=6.6

k=6.6

b=

h=5.6

k=5.6

b=

h=5.2

k=5.2

b=

h=4.8

k=4.8

b=

h=4.4

k=4.4

b=

h=4

k=4

b=

1

h=8.0

k=8.0

b=

h=7.0

k=7.0

b=

h=6.6

k=6.6

b=

h=6.2

k=6.2

b=

h=5.8

k=5.8

b=

h=5.4

k=5.4

b=

h=5

k=5

b=

r/c 1 2 3 4 5 6 7

(6,6) 1

(7,5) 1

(6,5) 1.4

State k

6

h=6

k=6

b=

h=5

k=5

b=

h=4

k=4

b=

h=3

k=3

b=

h=2

k=2

b=(6,6)

h=1

k=1

b=(7,6)

h=0

k=0

b=

5

h=6.4

k=6.4

b=

h=5.4

k=5.4

b=

h=4.4

k=4.4

b=

h=3.4

k=3.4

b=

h=2.4

k=2.4

b=(6,6)

h=1.4

k=1.4

b=(7,6)

h=1

k=1

b=(7,6)

4

h=6.8

k=6.8

b=

h=5.8

k=5.8

b=

h=4.8

k=4.8

b=

h=3.8

k=3.8

b=

h=2.8

k=2.8

b=

h=2.4

k=2.4

b=

h=2

k=2

b=

3

h=7.2

k=7.2

b=

h=6.2

k=6.2

b=

h=5.2

k=5.2

b=

h=4.2

k=4.2

b=

h=3.8

k=3.8

b=

h=3.4

k=3.4

b=

h=3

k=3

b=

2

h=7.6

k=7.6

b=

h=6.6

k=6.6

b=

h=5.6

k=5.6

b=

h=5.2

k=5.3

b=

h=4.8

k=4.8

b=

h=4.4

k=4.4

b=

h=4

k=4

b=

1

h=8.0

k=8.0

b=

h=7.0

k=7.0

b=

h=6.6

k=6.6

b=

h=6.2

k=6.2

b=

h=5.8

k=5.8

b=

h=5.4

k=5.4

b=

h=5

k=5

b=

r/c 1 2 3 4 5 6 7

(7,5) 1

(6,5) 1.4

(5,6) 2

(5,5) 2.4

State k

6

h=6

k=6

b(1,6)=

h=5

k=5

b=

h=4

k=4

b=

h=3

k=3

b=

h=2

k=2

b=(6,6)

h=1

k=1

b=(7,6)

h=0

k=0

b=

5

h=6.4

k=6.4

b(1,5)=

h=5.4

k=5.4

b=

h=4.4

k=4.4

b=

h=3.4

k=3.4

b=

h=2.4

k=2.4

b=(6,6)

h=1.4

k=1.4

b=(7,6)

h=1

k=1

b=(7,6)

4

h=6.8

k=6.8

b(1,4)=

h=5.8

k=5.8

b=

h=4.8

k=4.8

b=

h=3.8

k=3.8

b=

h=2.8

k=2.8

b=

h=2.4

k=2.4

b=(7,5)

h=2

k=2

b=(7,5)

3

h=7.2

k=7.2

b(1,3)=

h=6.2

k=6.2

b=

h=5.2

k=5.2

b=

h=4.2

k=4.2

b=

h=3.8

k=3.8

b=

h=3.4

k=3.4

b=

h=3

k=3

b=

2

h=7.6

k=7.6

b(1,2)=

h=6.6

k=6.6

b=

h=5.6

k=5.6

b=

h=5.2

k=

b=

h=4.8

k=4.8

b=

h=4.4

k=4.4

b=

h=4

k=4

b=

1

h=8.0

k=8.0

b(1,1)=

h=7.0

k=7.0

b=

h=6.6

k=6.6

b=

h=6.2

k=6.2

b=

h=5.8

k=5.8

b=

h=5.4

k=5.4

b=

h=5

k=5

b=

r/c 1 2 3 4 5 6 7

(6,5) 1.4

(5,6) 2

(7,4) 2

(6,4) 2.4

(5,5) 2.4

State k

6

h=6

k=6

b(1,6)=

h=5

k=

b=

h=10003

k=4

b=(4,6)

h=3

k=3

b=(5,6)

h=2

k=2

b=(6,6)

h=1

k=1

b=(7,6)

h=0

k=0

b=

5

h=6.4

k=6.4

b(1,5)=

h=5.4

k=

b=

h=10003.4

k=4.4

b=(4,6)

h=3.4

k=3.4

b=(5,6)

h=2.4

k=2.4

b=(6,6)

h=1.4

k=1.4

b=(7,6)

h=1

k=1

b=(7,6)

4

h=6.8

k=6.8

b(1,4)=

h=5.8

k=

b=

h=4.8

k=

b=

h=3.8

k=3.8

b=(5,5)

h=2.8

k=2.8

b=(6,5)

h=2.4

k=2.4

b=(7,5)

h=2

k=2

b=(7,5)

3

h=7.2

k=7.2

b(1,3)=

h=6.2

k=

b=

h=5.2

k=

b=

h=4.2

k=4.2

b=(5,4)

h=3.8

k=3.8

b=(6,4)

h=3.4

k=3.4

b=(7,4)

h=3

k=3

b=(7,4)

2

h=7.6

k=7.6

b(1,2)=

h=6.6

k=6.6

b=

h=5.6

k=5.6

b=

h=5.2

k=5.2

b=

h=4.8

k=4.8

b=

h=4.4

k=4.4

b=

h=4

k=4

b=

1

h=8.0

k=8.0

b(1,1)=

h=7.0

k=7.0

b=

h=6.6

k=6.6

b=

h=6.2

k=6.2

b=

h=5.8

k=5.8

b=

h=5.4

k=5.4

b=

h=5

k=5

b=

r/c 1 2 3 4 5 6 7

(7,3) 3

(6,3) 3.4

(4,5) 3.4

(5,3) 3.8

(4,4) 3.8

(3,6) 4

(4,3) 4.2

(3,5) 4.4

State k

6

h=20004

k=6

b=(2,6)

h=10004

k=5

b=(3,6)

h=10003

k=4

b=(4,6)

h=3

k=3

b=(5,6)

h=2

k=2

b=(6,6)

h=1

k=1

b=(7,6)

h=0

k=0

b=

5

h=20004.4

k=6.4

b=(2,6)

h=10004.4

k=5.4

b=(3,6)

h=10003.4

k=4.4

b=(4,6)

h=3.4

k=3.4

b=(5,6)

h=2.4

k=2.4

b=(6,6)

h=1.4

k=1.4

b=(7,6)

h=1

k=1

b=(7,6)

4

h=20006.8

k=6.8

b=(2,5)

h=10004.8

k=5.8

b=(3,5)

h=10003.8

k=10003.8

b=(4,5)

h=3.8

k=3.8

b=(5,5)

h=2.8

k=2.8

b=(6,5)

h=2.4

k=2.4

b=(7,5)

h=2

k=2

b=(7,5)

3

h=8.0

k=8.0

b=(2,2)

h=10005.2

k=6.2

b=(3,4)

h=10004.2

k=5.2

b=(4,4)

h=4.2

k=4.2

b=(5,4)

h=3.8

k=3.8

b=(6,4)

h=3.4

k=3.4

b=(7,4)

h=3

k=3

b=(7,4)

2

h=7.6

k=7.6

b=(2,2)

h=6.6

k=6.6

b=(3,2)

h=5.6

k=5.6

b=(4.3)

h=10004.2

k=5.2

b=(5,3)

h=4.8

k=4.8

b=(6,3)

h=4.4

k=4.4

b=(7,3)

h=4

k=4

b=(7,3)

1

h=8.0

k=8.0

b=(2,2)

h=7.0

k=7.0

b=(3,2)

h=6.6

k=6.6

b=(3,2)

h=6.2

k=6.2

b=(5,2)

h=5.8

k=5.8

b=(6,2)

h=5.4

k=5.4

b=(7,2)

h=5

k=5

b=(7,2)

r/c 1 2 3 4 5 6 7

(1,6) 6

(1,5) 6.4

(1,4) 6.8

(1,2) 7.6

(1,3) 8.0

(1,1) 8.0

State k

6

h=20004

k=6

b=(2,6)

h=10004

k=5

b=(3,6)

h=10003

k=4

b=(4,6)

h=3

k=3

b=(5,6)

h=2

k=2

b=(6,6)

h=1

k=1

b=(7,6)

h=0

k=0

b=

5

h=20004.4

k=6.4

b=(2,6)

h=10004.4

k=5.4

b=(3,6)

h=10003.4

k=4.4

b=(4,6)

h=3.4

k=3.4

b=(5,6)

h=2.4

k=2.4

b=(6,6)

h=1.4

k=1.4

b=(7,6)

h=1

k=1

b=(7,6)

4

h=20006.8

k=6.8

b=(2,5)

h=10004.8

k=5.8

b=(3,5)

h=10003.8

k=4.8

b=(4,5)

h=3.8

k=3.8

b=(5,5)

h=2.8

k=2.8

b=(6,5)

h=2.4

k=2.4

b=(7,5)

h=2

k=2

b=(7,5)

3

h=8.0

k=8.0

b=(2,2)

h=10005.2

k=6.2

b=(3,4)

h=10004.2

k=5.2

b=(4,4)

h=4.2

k=4.2

b=(5,4)

h=3.8

k=3.8

b=(6,4)

h=3.4

k=3.4

b=(7,4)

h=3

k=3

b=(7,4)

2

h=7.6

k=7.6

b=(2,2)

h=6.6

k=6.6

b=(3,2)

h=5.6

k=5.6

b=(4.3)

h=10004.2

k=5.2

b

h=4.8

k=4.8

b=(6,3)

h=4.4

k=4.4

b=(7,3)

h=4

k=4

b=(7,3)

1

h=8.0

k=8.0

b=(2,2)

h=7.0

k=7.0

b=(3,2)

h=6.6

k=6.6

b=(3,2)

h=6.2

k=6.2

b=(5,2)

h=5.8

k=5.8

b=(6,2)

h=5.4

k=5.4

b=(7,2)

h=5

k=5

b=(7,2)

r/c 1 2 3 4 5 6 7

(1,6) 6

(1,5) 6.4

(1,4) 6.8

(1,2) 7.6

(1,3) 8.0

(1,1) 8.0

State k

6

h=20004

k=6

b=(2,6)

h=10004

k=5

b=(3,6)

h=10003

k=4

b=(4,6)

h=3

k=3

b=(5,6)

h=2

k=2

b=(6,6)

h=1

k=1

b=(7,6)

h=0

k=0

b=

5

h=20004.4

k=6.4

b=(2,6)

h=10004.4

k=5.4

b=(3,6)

h=10003.4

k=4.4

b=(4,6)

h=3.4

k=3.4

b=(5,6)

h=2.4

k=2.4

b=(6,6)

h=1.4

k=1.4

b=(7,6)

h=1

k=1

b=(7,6)

4

h=20006.8

k=6.8

b=(2,5)

h=10004.8

k=5.8

b=(3,5)

h=10003.8

k=4.8

b=(4,5)

h=3.8

k=3.8

b=(5,5)

h=2.8

k=2.8

b=(6,5)

h=2.4

k=2.4

b=(7,5)

h=2

k=2

b=(7,5)

3

h=8.0

k=8.0

b=(2,2)

h=10005.2

k=6.2

b=(3,4)

h=10004.2

k=5.2

b=(4,4)

h=4.2

k=4.2

b=(5,4)

h=3.8

k=3.8

b=(6,4)

h=3.4

k=3.4

b=(7,4)

h=3

k=3

b

2

h=7.6

k=7.6

b=(2,2)

h=6.6

k=6.6

b=(3,2)

h=5.6

k=5.6

b=(4.3)

h=10004.2

k=5.2

b=(5,3)

h=4.8

k=4.8

b=(6,3)

h=4.4

k=4.4

b=(7,3)

h=4

k=4

b=(7,3)

1

h=8.0

k=8.0

b=(2,2)

h=7.0

k=7.0

b=(3,2)

h=6.6

k=6.6

b=(3,2)

h=6.2

k=6.2

b=(5,2)

h=5.8

k=5.8

b=(6,2)

h=5.4

k=5.4

b=(7,2)

h=5

k=5

b=(7,2)

r/c 1 2 3 4 5 6 7

(1,6) 6

(1,5) 6.4

(1,4) 6.8

(1,2) 7.6

(1,3) 8.0

(1,1) 8.0

State k

(4,3) 4.2

(1,6) 6

(1,5) 6.4

(1,4) 6.8

(1,2) 7.6

(1,3) 8.0

(1,1) 8.0

State k

6

h=20004

k=6

b=(2,6)

h=10004

k=5

b=(3,6)

h=10003

k=4

b=(4,6)

h=3

k=3

b=(5,6)

h=2

k=2

b=(6,6)

h=1

k=1

b=(7,6)

h=0

k=0

b=

5

h=20004.4

k=6.4

b=(2,6)

h=10004.4

k=5.4

b=(3,6)

h=10003.4

k=4.4

b=(4,6)

h=3.4

k=3.4

b=(5,6)

h=2.4

k=2.4

b=(6,6)

h=1.4

k=1.4

b=(7,6)

h=1

k=1

b=(7,6)

4

h=20006.8

k=6.8

b=(2,5)

h=10004.8

k=5.8

b=(3,5)

h=10003.8

k=4.8

b=(4,5)

h=3.8

k=3.8

b=(5,5)

h=2.8

k=2.8

b=(6,5)

h=2.4

k=2.4

b=(7,5)

h=2

k=2

b=(7,5)

3

h=8.0

k=8.0

b=(2,2)

h=10005.2

k=6.2

b

h=10004.2

k=5.2

b=(4,4)

h=4.2

k=4.2

b=(5,4)

h=3.8

k=3.8

b=(6,4)

h=3.4

k=3.4

b=(7,4)

h=3

k=3

b=(7,4)

2

h=7.6

k=7.6

b=(2,2)

h=6.6

k=6.6

b=(3,2)

h=5.6

k=5.6

b=(4.3)

h=10004.2

k=5.2

b=(5,3)

h=4.8

k=4.8

b=(6,3)

h=4.4

k=4.4

b=(7,3)

h=4

k=4

b=(7,3)

1

h=8.0

k=8.0

b=(2,2)

h=7.0

k=7.0

b=(3,2)

h=6.6

k=6.6

b=(3,2)

h=6.2

k=6.2

b=(5,2)

h=5.8

k=5.8

b=(6,2)

h=5.4

k=5.4

b=(7,2)

h=5

k=5

b=(7,2)

r/c 1 2 3 4 5 6 7

6

h=20004

k=6

b=(2,6)

h=10004

k=5

b

h=10003

k=4

b

h=3

k=3

b=(5,6)

h=2

k=2

b=(6,6)

h=1

k=1

b=(7,6)

h=0

k=0

b=

5

h=20004.4

k=6.4

b=(2,6)

h=10004.4

k=5.4

b=(3,6)

h=10003.4

k=4.4

b=(4,6)

h=3.4

k=3.4

b=(5,6)

h=2.4

k=2.4

b=(6,6)

h=1.4

k=1.4

b=(7,6)

h=1

k=1

b=(7,6)

4

h=20006.8

k=6.8

b=(2,5)

h=10004.8

k=5.8

b=(3,5)

h=10003.8

k=4.8

b=(4,5)

h=3.8

k=3.8

b=(5,5)

h=2.8

k=2.8

b=(6,5)

h=2.4

k=2.4

b=(7,5)

h=2

k=2

b=(7,5)

3

h=8.0

k=8.0

b=(2,2)

h=10005.2

k=6.2

b=(3,4)

h=10004.2

k=5.2

b=(4,4)

h=4.2

k=4.2

b=(5,4)

h=3.8

k=3.8

b=(6,4)

h=3.4

k=3.4

b=(7,4)

h=3

k=3

b=(7,4)

2

h=7.6

k=7.6

b=(2,2)

h=6.6

k=6.6

b=(3,2)

h=10004.6

k=5.6

b=(4.3)

h=10004.2

k=5.2

b=(5,3)

h=4.8

k=4.8

b=(6,3)

h=4.4

k=4.4

b=(7,3)

h=4

k=4

b=(7,3)

1

h=8.0

k=8.0

b=(2,2)

h=7.0

k=7.0

b=(3,2)

h=6.6

k=6.6

b=(3,2)

h=6.2

k=6.2

b=(5,2)

h=5.8

k=5.8

b=(6,2)

h=5.4

k=5.4

b=(7,2)

h=5

k=5

b=(7,2)

r/c 1 2 3 4 5 6 7

(3,2) 5.6

(1,6) 6

(1,5) 6.4

(1,4) 6.8

(1,2) 7.6

(1,3) 8.0

(1,1) 8.0

State k

6

h=20004

k=6

b=(2,6)

h=10004

k=5

b

h=10003

k=4

b=(4,6)

h=3

k=3

b=(5,6)

h=2

k=2

b=(6,6)

h=1

k=1

b=(7,6)

h=0

k=0

b=

5

h=20004.4

k=6.4

b=(2,6)

h=10004.4

k=5.4

b=(3,6)

h=10003.4

k=4.4

b=(4,6)

h=3.4

k=3.4

b=(5,6)

h=2.4

k=2.4

b=(6,6)

h=1.4

k=1.4

b=(7,6)

h=1

k=1

b=(7,6)

4

h=20006.8

k=6.8

b=(2,5)

h=10004.8

k=5.8

b=(3,5)

h=10003.8

k=4.8

b=(4,5)

h=3.8

k=3.8

b=(5,5)

h=2.8

k=2.8

b=(6,5)

h=2.4

k=2.4

b=(7,5)

h=2

k=2

b=(7,5)

3

h=8.0

k=8.0

b=(2,2)

h=10005.2

k=6.2

b=(3,4)

h=10004.2

k=5.2

b=(4,4)

h=4.2

k=4.2

b=(5,4)

h=3.8

k=3.8

b=(6,4)

h=3.4

k=3.4

b=(7,4)

h=3

k=3

b=(7,4)

2

h=7.6

k=7.6

b=(2,2)

h=10005.6

k=6.6

b=(3,2)

h=10004.6

k=5.6

b=(4.3)

h=10004.2

k=5.2

b=(5,3)

h=4.8

k=4.8

b=(6,3)

h=4.4

k=4.4

b=(7,3)

h=4

k=4

b=(7,3)

1

h=8.0

k=8.0

b=(2,2)

h=10006.0

k=7.0

b=(3,2)

h=10005.6

k=6.6

b=(3,2)

h=6.2

k=6.2

b=(5,2)

h=5.8

k=5.8

b=(6,2)

h=5.4

k=5.4

b=(7,2)

h=5

k=5

b=(7,2)

r/c 1 2 3 4 5 6 7

(1,6) 6(4,1) 6.2(1,5) 6.4(3,1) 6.6(2,2) 6.6(1,4) 6.8(2,1) 7.0(1,2) 7.6(1,3) 8.0(1,1) 8.0

State k

6

h=20004

k=6

b=(2,6)

h=10004

k=5

b

h=10003

k=4

b=(4,6)

h=3

k=3

b=(5,6)

h=2

k=2

b=(6,6)

h=1

k=1

b=(7,6)

h=0

k=0

b=

5

h=20004.4

k=6.4

b=(2,6)

h=10004.4

k=5.4

b=(3,6)

h=10003.4

k=4.4

b=(4,6)

h=3.4

k=3.4

b=(5,6)

h=2.4

k=2.4

b=(6,6)

h=1.4

k=1.4

b=(7,6)

h=1

k=1

b=(7,6)

4

h=20006.8

k=6.8

b=(2,5)

h=10004.8

k=5.8

b=(3,5)

h=10003.8

k=4.8

b=(4,5)

h=3.8

k=3.8

b=(5,5)

h=2.8

k=2.8

b=(6,5)

h=2.4

k=2.4

b=(7,5)

h=2

k=2

b=(7,5)

3

h=8.0

k=8.0

b=(2,2)

h=10005.2

k=6.2

b=(3,4)

h=10004.2

k=5.2

b=(4,4)

h=4.2

k=4.2

b=(5,4)

h=3.8

k=3.8

b=(6,4)

h=3.4

k=3.4

b=(7,4)

h=3

k=3

b=(7,4)

2

h=7.6

k=7.6

b=(2,2)

h=10005.6

k=6.6

b=(3,2)

h=10004.6

k=5.6

b=(4.3)

h=10004.2

k=5.2

b=(5,3)

h=4.8

k=4.8

b=(6,3)

h=4.4

k=4.4

b=(7,3)

h=4

k=4

b=(7,3)

1

h=8.0

k=8.0

b=(2,2)

h=10006.0

k=7.0

b=(3,2)

h=10005.6

k=6.6

b=(3,2)

h=6.2

k=6.2

b=(5,2)

h=5.8

k=5.8

b=(6,2)

h=5.4

k=5.4

b=(7,2)

h=5

k=5

b=(7,2)

r/c 1 2 3 4 5 6 7

(1,6) 6(4,1) 6.2(1,5) 6.4(3,1) 6.6(2,2) 6.6(1,4) 6.8(2,1) 7.0(1,2) 7.6(1,3) 8.0(1,1) 8.0

State k

6

h=20004

k=6

b=(2,6)

h=10004

k=5

b=(3,6)

h=10003

k=4

b=(4,6)

h=3

k=3

b=(5,6)

h=2

k=2

b=(6,6)

h=1

k=1

b=(7,6)

h=0

k=0

b=

5

h=20004.4

k=6.4

b=(2,6)

h=10004.4

k=5.4

b=(3,6)

h=10003.4

k=4.4

b=(4,6)

h=3.4

k=3.4

b=(5,6)

h=2.4

k=2.4

b=(6,6)

h=1.4

k=1.4

b=(7,6)

h=1

k=1

b=(7,6)

4

h=20006.8

k=6.8

b=(2,5)

h=10004.8

k=5.8

b)=(3,5)

h=10003.8

k=4.8

b=(4,5)

h=3.8

k=3.8

b=(5,5)

h=2.8

k=2.8

b=(6,5)

h=2.4

k=2.4

b=(7,5)

h=2

k=2

b=(7,5)

3

h=8.0

k=8.0

b=(2,2)

h=10005.2

k=6.2

b=(3,4)

h=10004.2

k=5.2

b=(4,4)

h=4.2

k=4.2

b=(5,4)

h=3.8

k=3.8

b=(6,4)

h=3.4

k=3.4

b=(7,4)

h=3

k=3

b=(7,4)

2

h=7.6

k=7.6

b=(2,2)

h=10005.6

k=6.6

b=(3,2)

h=10004.6

k=5.6

b=(4.3)

h=10004.2

k=5.2

b=(5,3)

h=4.8

k=4.8

b=(6,3)

h=4.4

k=4.4

b=(7,3)

h=4

k=4

b=(7,3)

1

h=8.0

k=8.0

b=(2,2)

h=10006.0

k=7.0

b=(3,2)

h=10005.6

k=6.6

b=(3,2)

h=6.2

k=6.2

b=(5,2)

h=5.8

k=5.8

b=(6,2)

h=5.4

k=5.4

b=(7,2)

h=5

k=5

b=(7,2)

r/c 1 2 3 4 5 6 7

(4,1) 6.2(1,5) 6.4(3,1) 6.6(2,2) 6.6(1,4) 6.8(2,1) 7.0(1,2) 7.6(1,3) 8.0(1,1) 8.0

State k

6

h=20004

k=6

b=(2,6)

h=10004

k=5

b=(3,6)

h=10003

k=4

b=(4,6)

h=3

k=3

b=(5,6)

h=2

k=2

b=(6,6)

h=1

k=1

b=(7,6)

h=0

k=0

b=

5

h=20004.4

k=6.4

b=(2,6)

h=10004.4

k=5.4

b=(3,6)

h=10003.4

k=4.4

b=(4,6)

h=3.4

k=3.4

b=(5,6)

h=2.4

k=2.4

b=(6,6)

h=1.4

k=1.4

b=(7,6)

h=1

k=1

b=(7,6)

4

h=20006.8

k=6.8

b=(2,5)

h=10004.8

k=5.8

b=(3,5)

h=10003.8

k=4.8

b=(4,5)

h=3.8

k=3.8

b=(5,5)

h=2.8

k=2.8

b=(6,5)

h=2.4

k=2.4

b=(7,5)

h=2

k=2

b=(7,5)

3

h=8.0

k=8.0

b=(2,2)

h=10005.2

k=6.2

b=(3,4)

h=10004.2

k=5.2

b=(4,4)

h=4.2

k=4.2

b=(5,4)

h=3.8

k=3.8

b=(6,4)

h=3.4

k=3.4

b=(7,4)

h=3

k=3

b=(7,4)

2

h=7.6

k=7.6

b=(2,2)

h=10005.6

k=6.6

b=(3,2)

h=7.6

k=7.6

b=(4.1)

h=10004.2

k=5.2

b=(5,3)

h=4.8

k=4.8

b=(6,3)

h=4.4

k=4.4

b=(7,3)

h=4

k=4

b=(7,3)

1

h=8.0

k=8.0

b=(2,2)

h=10006.0

k=7.0

b=(3,2)

h=7.2

k=7.2

b=(4,1)

h=6.2

k=6.2

b=(5,2)

h=5.8

k=5.8

b=(6,2)

h=5.4

k=5.4

b=(7,2)

h=5

k=5

b=(7,2)

r/c 1 2 3 4 5 6 7

(5,2) 4.8(5,1) 5.8(3,2) 5.6(1,5) 6.4(3,1) 6.6(2,2) 6.6(1,4) 6.8(2,1) 7.0(1,2) 7.6(1,3) 8.0(1,1) 8.0

State k

6

h=20004

k=6

b=(2,6)

h=10004

k=5

b=(3,6)

h=10003

k=4

b=(4,6)

h=3

k=3

b=(5,6)

h=2

k=2

b=(6,6)

h=1

k=1

b=(7,6)

h=0

k=0

b=

5

h=20004.4

k=6.4

b=(2,6)

h=10004.4

k=5.4

b=(3,6)

h=10003.4

k=4.4

b=(4,6)

h=3.4

k=3.4

b=(5,6)

h=2.4

k=2.4

b=(6,6)

h=1.4

k=1.4

b=(7,6)

h=1

k=1

b=(7,6)

4

h=20006.8

k=6.8

b=(2,5)

h=10004.8

k=5.8

b=(3,5)

h=10003.8

k=4.8

b=(4,5)

h=3.8

k=3.8

b=(5,5)

h=2.8

k=2.8

b=(6,5)

h=2.4

k=2.4

b=(7,5)

h=2

k=2

b=(7,5)

3

h=8.0

k=8.0

b=(2,2)

h=10005.2

k=6.2

b=(3,4)

h=10004.2

k=5.2

b=(4,4)

h=4.2

k=4.2

b=(5,4)

h=3.8

k=3.8

b=(6,4)

h=3.4

k=3.4

b=(7,4)

h=3

k=3

b=(7,4)

2

h=7.6

k=7.6

b=(2,2)

h=10005.6

k=6.6

b=(3,2)

h=7.6

k=7.6

b=(4.1)

h=10004.2

k=5.2

b=(5,3)

h=4.8

k=4.8

b=(6,3)

h=4.4

k=4.4

b=(7,3)

h=4

k=4

b=(7,3)

1

h=8.0

k=8.0

b=(2,2)

h=10006.0

k=7.0

b=(3,2)

h=7.2

k=7.2

b=(4,1)

h=6.2

k=6.2

b=(5,2)

h=5.8

k=5.8

b=(6,2)

h=5.4

k=5.4

b=(7,2)

h=5

k=5

b=(7,2)

r/c 1 2 3 4 5 6 7

Exam 1

Exam 2

My Current Work

Compared with A* replanner in our problem, D* performance superior over A* replanner

0

50

100

150

200

250

300

Exam 1 Exam 2

A* repl annerD*

Checking points per replanning

Future work and related work Modify program, make it more robust with more ex

periments, speed up with more modifications. D* Limitation

– D* search from goal configuration, what if there are several goal configurations (it’s common in manipulator motion planning)?

– When the goal object is moving– Current on-line planning methods using A* based techni

ques focus on multi-directional search and parallel planning ([Dominik HENRICH, Christian WURLL and Heinz WÖRN, 1998], etc )

– D* should be adapted for our problems

Future work and related work

– Consult other D*-like replanning algorithms (e.g D* Lite [Koenig, 2002] )

Survey other real-time motion planning techniques in high dimensional C-Space

– Decomposition-based methods ([Kavraki, 2001], [Me

diavilla, 2002], etc)– Probabilistic roadmap based methods(most de

al with static environment)

Future work and related work

Use a more general 3D model to represent manipulator and obstacles

– Hierarchy structure– Tree structure

Future work and related work

– Taxonomy

Future work and related work

Experiment using real robot arm: a challenging work

Images from cameras Model parameters

Computer vision techniquesMotion planning

C-Space Visualization for Teleoperation

Applications of C-Space Visualization– Provide important qualitative information for

mechanical design (E.Sacks, C.Pisula and L.Joskowicz “Visualizing 3D Configuration Spaces for Mechanical Design.” ).

– Evaluation of path planning methods– Teleoperation (I.Ivanisevic and J.Lumelsky “Configuration Spa

ce as a Means for Augmenting Human Performance in Teleoperation Tasks.” IEEE Trans.Syst.Man,Cyber.,vol.30,pp.471-484,Jun.2000).

C-Space Visualization for Teleoperation

It’s easier for humans to handle motion planning problems in C-Space than in W-Space

C-Space Visualization for Teleoperation

Challenges– When the computer which generates C-Space data is

not the same as the computer which receives humans input, C-Space data must be transfered through network

– C-Space data are too large• 161*71*121 for my current implementation

– C-Space data change caused by dynamic environment, etc

– Poor network bandwidth

C-Space Visualization for Teleoperation

So, C-Space data compression is necessary Additional work

C-Space for a Cylinder Object

C-Space Data

3D Models Data

3D Model Data Compression

Framework ：