1

OperationalSpaceFramework! .. motion in contact

Whole-body Compliance!

Joint Space Control Joint Space Control

Joint Space Control

F

( )GoalV xF

( )GoalV x

T FJ

Task-Oriented Control

1 2

3 4

5 6

2

F

dynamics( )F F

x

x Fp

Task-Oriented Dynamics Unified Motion & Force Control

motion contactF F F contactF

motionF

Equations of Motion

d L LF

dt x x

with ( , ) ( , ) ( )L x x T x x V x

End-Effector Control

( )TJ q F F

1

2

T

goal p g gV k x x x x

( )GoalV xF

1

2

T

goal p g gU k x x x x

System T Vd T

Fdt x x

ˆ

goalF V VX

Passive Systems

0

goalT Vd T

dt x x

StableConservative Forces

Asymptotic Stability

is asymptotically stable if

0 ; 0TsF x for x

0s v vF k x k

ˆp g vF k x x k x p Control

s

goalT Vd T

dt x xF

a system

sFx

7 8

9 10

11 12

3

Artificial Potential Field

( ) ( , ) ( )x x x x p x F

Operational Space Dynamics

End-Effector Centrifugal and Coriolis forces

( , ) :x x

( ) :p x End-Effector Gravity forces:F End-Effector Generalized forces

( ) : x End-Effector Kinetic Energy Matrix

:x End-Effector Position andOrientation

Example2-d.o.f arm

ˆ ( )p g vF k x x k x p x

( ) ( , ) ( )x x x x p x F 1q1l

2q2l

Closed loop behavior

111*

1( ) v p gm q x k x k mx yx

122*

2( ) v p gm q y k y k my xy

* 2 *1 2 1 112 p g vm c m x m y k x x k x

* 2 *1 2 1 212 p g vm c m y m x k y y k y

( ) ( , ) ( )A q q b q q g q

Joint Space Dynamics

Centrifugal and Coriolis forces( , ) :b q q( ) :g q Gravity forces: Generalized forces

( ) :A q Kinetic Energy Matrix

:q Joint Coordinates

Lagrange Equations

( )d L L

dt q q

( ) ( , ) ( )A q q b q q g q

(( )) ,A b q qq 1

2( ): TA q qAqT

13 14

15 16

17 18

4

Equations of Motionvcii

Pci

Link i

TTotal Kinetic Energy:

1

2Lin i

T

kAqT T q

Equations of Motion

1( )

2 i i

T T Ci i C C i i iT mv v I

1

n

ii

T T

vcii

Pci

Link i

Explicit Form

Total Kinetic Energy

Equations of Motion

Generalized Coordinates q

Kinetic EnergyQuadratic Form of

Generalized Velocities

1

1(

1

2)

2 i i

nT T C

i C C i iT

ii

m v vq A q I

q

1

2Tq qT A

vcii

Pci

Link i

Explicit Form

Generalized Velocities

Equations of Motion

i ivC Jv q

1

1( )

2 i i i i

T Tv v

nT T C

i ii

m q q qJ J J JI q

Explicit Formvcii

Pci

Link i

iiC J q

1

1(

1

2)

2 i i

nT T C

i C C i iT

ii

m v vq A q I

Equations of Motionvcii

Pci

Link i

Explicit Form

1

2Tq qA

1

( )1

2 i i i i

nT T

i vi

T Cv imq J J I J qJ

1

( )i i i i

nT T C

i v v ii

A m J J J I J

( )

11 12 1

21 22 2

1 2

( )n n

n

n

n n nn

a a a

a a aA q

a a a

19 20

21 22

23 24

5

Christoffel Symbols1

( )2ijk ijk ikj jkib a a a

ij

k

a

q

2( , ) ( ) ( )b q q C q q B q qq

B

b b b

b b b

b b b

q q

q q

q qn

n n n n

n n

n n

n n n n n n n

( ) [ ]

(

( )) (

( ))

, , ,(

, , ,(

, , ,( (

q qq

L

N

MMMM

O

Q

PPPP

L

N

MMMM

O

Q

PPPP

1

2

1

21

2 2 2

2 2 2

2 2 2

1 12 1 13 1 1)

2 12 2 13 2 1)

12 13 1)

1 2

1 3

1)

C

b b b

b b b

b b b

q

q

qn n n

nn

nn

n n n nn n

( )[ ]

( ) ( )

, , ,

, , ,

, , ,

q q

L

N

MMMM

O

Q

PPPP

L

N

MMMM

O

Q

PPPP2

1 11 1 22 1

2 11 2 22 2

11 22

12

22

2

1

Gravity Vector

m2g

c2

m1g

c1

mng

cnm3g

c3

1 21 2( ( ) ( ) ( ))n

T T Tv v v ng J m g J m g J m g

Effector Equations of Motion

Non-Redundant Manipulator ; 0n m

01 2

T

mx x x x

1 2 T

nq q q q x G q

0R1nR

10n

Domain

1

,n

q i ii

D q q

x1Cq

2Cq

G

x qD G D

x qD G D

In form a complete set of configuration parameters for the manipulator.01 2, , , x mD x x x

Excluding Singularitiesand such that G is one-to-one

:qD

system of generalizedcoordinates

01 , , : mx x

Kinetic Energy

1, ( )

2TT x x x x x

Kinetic Energy Matrix0 0

( ) :m m x

25 26

27 28

29 30

6

1( , ) ( )

2T

xT x x x x x

Identity( , ) ( , )x qT x x T q q

1 1( ) ( )

2 2T Tx X x q A q q

x Jq 1( ) ( ) ( ) ( )Tx J q A q J q

( ) ( )Tp x J g q

1

2TT

x x xx x

d Tx x

dt x

1

2 i

Tx

i

Tx x

x

System T Ud T

Fdt x x

1

0

1

2,

1

2 m

Tx

x

x x

x x x

x x

( , ) ( , )x x x m x x 11

;2 i

T Ti xm x x J A J

( , )

( , ) ( , )

T T

T T

x J Aq h q q J Aq

m x x J l q q J Aq

where h Jq1

2

i

Ti ql q A q

1 1

1 2

1( , )

21 1

( , ) | | 2 | |2 21

( , ) | |2

( ) . | |

1| | ( , )

2

( , ) ( , )

i

i i

i

i i

i

Ti x

T T T Tx x

T Tx

T Tnx q

i i i

T Tx

T T

m x x x x

m x x x J A J x x J A J x

m x x q A q J A q

q q qq A q q A q

x x x

q A q J l q q

m x x J l q q J A q

( , ) ( , )TJ Aq l q q h q q

( ) ( , ) ( , )TJ q b q q h q q

where h Jq1

2

i

Ti ql q A q

31 32

33 34

35 36

7

Joint Space/Operational SpaceRelationships

( , ) ( , ) x qT x x T q q

1 1( ) ( )

2 2T Tx X x q A q q

1 1

2 2T TTq qJ J Aq q

Using ( )x J q q

where ( , ) ( )h q q J q q

1( ) ( ) ( ) ( )Tx J q A q J q

( , ) ( ) ( , ) ( ) ( , )Tx x J q b q q q h q q

( ) ( ) ( )Tp x J q g q

Joint Space/Operational SpaceRelationships

, , and are all expressed in terms ofjoint coordinates P

x qD G D

The domain can be extended toxD

:qD domain excluding singularitiesqD

Example

2

2

1

1

d cx

d s

20

2

1 1

1 1

d s cJ

d c s

2 2q d

g

l1y

CI1

CI2

m1

m2

d2

x

20

2

1 1

1 1

d s cJ

d c s

21 1 0 1;

1 0

dJ

11 21

2 22

0 1 0 0 1

1 0 0 1 0

m d

d m

0

2

0 11 1

01 1

c sJ

ds c

1 J

21

2 2

0

0

m

m m

2221 222 1 1

2 22

I I m l

md

2 2m m

1x1y

1m

2m

2m

37 38

39 40

41 42

8

20

2

01 1 1 1

1 1 1 10

mc s c s

s c s cm

2 2 2m m m

20 2 2 2

22 2 2

1 1

1 1

m m s m s c

m s c m m c

2m2 2m m

20 2 2 2

22 2 2

1 1

1 1

m m s m s c

m s c m m c

Nonlinear Dynamic Decoupling

with TJ F

( ) ( , ) ( )x x x x p x F Model

*( ) ( ,ˆ ˆ) ( )ˆF px x x xF Control Structure

*I x FDecoupled System

Dynamic Decoupling

( ) ( , ) ( )x x x x p x F

*ˆ ˆ ˆ( , ) ( )F F x x p x

0

1 * 1 1ˆ ˆˆ mI X F P P

( )G x ( , )x x ( )P x

0

*( ) ( , ) ( )mI x G x F x x d t

1 ˆ( )G x I

1( , )x x P

( ) :d t unmodeled disturbances

Perfect Estimates

0

*mI x F

input of decoupled end-effector*F

Goal Position Control

*v p gF k x k x x

Closed Loop

0m v p p gI x k x k x k x

43 44

45 46

47 48

9

Closed Loop0m v p p gI x k x k x k x

t

x

max

2gx

1gx

PD Control

*v p gF k x k x x

Velocity-Like Control

* pv g

v

kF k x x x

k

dx

pd g

v

kx x x

k

dx

* pv g

v

kF k x x x

k

* v dF k x x

withmax

d

Vsat

x

1

0 1 max

dx

1

( ) 1

x if xsat x

sign x if x

Trajectory TrackingTrajectory: , , d d dx x x

0

* ( ) ( ) m d v d p dF I x k x x k x x

( ) ( ) ( ) d v d p dx x k x x k x x

with

or

X dx x

0 X v X p Xk k

In joint space

0 q v q p qk k

with q dq q

49 50

51 52

53 54

10

Compliant MotionControl Compliant Manipulation Primitives

Advanced Manipulation Capabilities

Multi-contact Manipulation

55 56

57 58

59 60

11

Unified Motion/Force Control

motion contactF F F contactF

motionF

Unified Motion/Force Control

0 0 0( ) , ( ) contactox x p x F F

•Generalized Selection Matrix

•Dynamic Model (Homogeneity)

Task Description Task Specification

motion forceF F F

1 0 0

0 1 0 ;

0 0 0

I

Selection matrix

Generalized Selection Matrix

fR

fRSelection in0( ) ( )fR f Rf R f

fRf

0RSelection in

f

Tf RR f

0RTf fR R f

0 0

0 0 ;

0 0

x

y

z

F

F F

F

3F FI

Generalized Selection Matrix

0 0

0 0 ;

0 0

x

y

z

M

M M

M

3M MI

61 62

63 64

65 66

12

0

0

TF F F

TM M M

R R

R R

0

0

TF F F

TM M M

R R

R R

Generalized Selection Matrix Basic Dynamic Model

( )x J q q

Operational force

?

Forces &Moments

( )TJ q F

Linear & AngularVelocities

0 ( )v

J q q

0( )x J E Jq qx

0 ˆf

Fm

TfE F

m

0 )(T TTJ JF FE

vx E

Forces & Moments

ˆv

Basic Dynamic Model

0TE E

( , ) ( )x x x p x F

0 0 0 0( , ) ( )x p x F

TE

ˆv

with

r rx E

Orientation Representation

rx

rdxr r rdx x x

r rx E

Instantaneous Angular Error

r rx E

r rx E

r rE x

Instantaneous Angular Error

67 68

69 70

71 72

13

p pdd

r rd

x xx x

x x

r rx E

r rx E

( )r r r d rx x x E

( )r r r dE x x

Control – Position Errors

p pdx x

ErrorVector

Goal Position pdd

rd

xx

x

*p p pd v pf k x x k x

*p vm k k

with r r r rdE x x x

Closed loop

. 0p v p p p pdI x k x k x x 0v pI k k

Direction Cosines

1

2T

r rE E

1 2 3

TT T Trx r r r

1 2 3

TT T Trd d d dx r r r

The angular rotation error

1 1 2 2 3 3

1ˆ ˆ ˆ

2 d d dr r r r r r

Euler ParametersThe end-effector orientation

0 1 2 3

T

rx

The desired orientation 0 1 2 3

T

d d d d d

The angular rotation error

1 0 3 2

2 3 0 1

3 2 1 0

2RE

( )R dE

Motion Tracking , ,pd pd pdx x x

*pd p p pd v p pdF x k x x k x x

Closed loop

. 0x v x p xI k k

px p pdx x with

Angular Acceleration

r rEx

r r rEx E

rr r rxE E E

73 74

75 76

77 78

14

Acceleration Direction Cosines

The orientation is described by

1 2 3

TT T Trx r r r

1 ( 1) 2 ( 1) 3 ( 1); ; ;n n nr x r y r z

0R1nR

10n

The second time derivatives

2

( 1)( 1) ( 1)2

nn n

d

dt

x

x x

2

( 1)( 1) ( 1)2

nn n

d

dt

y

y y

2

( 1)( 1) ( 1)2

nn n

d

dt

z

z z

However T T u v w u v w v w u

( ) , ) ( )( Tr r r rE R x x x x

This yields

where

1 3

2 3

3 3

, )(

T

Tr

T

r I

r I

r I

R

x

1 1, )

2 2(T T

d rd r rdE x R xx

Acceleration Direction Cosines

Euler ParametersThe acceleration associated with

Euler parameters

1 1( )

4 2T

since 0T

1 2 3

0 3 2

3 0 1

2 1 0

The angular acceleration vector

2T

The desired angular acceleration

2T

d d d

Euler Parameters

79 80

81 82

83 84

15

Motion Tracking , ,pd pd pdx x x

*pd p p pd v p pdF x k x x k x x

Closed loop

. 0x v x p xI k k

px p pdx x with

*d p v dm k k

with r r rdE x x

d r rd rdE x x

d r rd rd r rd rd dE x x E x E x and

Motion Tracking , ,pd pd pdx x x

*pd p p pd v p pdf x k x x k x x

Closed loop

0p pd v p pd p p pdx x k x x k x x

0d v d pk k

A Mass Spring SystemSystem

smz k z f

s sf k z

1s s

s

m f f fk

Control

fs f s d v sf f m k f f k f

s compf f m f

Control-loop System

0fs s v s s f s df k k f k k f f

Static Equilibrium

s df f

1s s

s

m f f fk

System End-Effector/Sensor System

0 0 0 0( , ) ( ) contactFx p x F

0 motion forceF F F

Unified Control

*0 0 0

ˆ ˆˆmotionmotion F PF

*0

ˆforceforc sensore F FF

85 86

87 88

89 90

16

End-Effector/Sensor System

0 0 0 0( , ) ( ) contactFx p x F

0 motion forceF F F

Unified Control

*0 0 0

ˆ ˆˆmotionmotion F PF

*0

ˆforf ce desorce iredF FF

Unified Motion & Force Control

Two decoupled Subsystems

*motionF *forceF

Unified Motion & Force Control

Two decoupled Subsystems

*motionF *forceF

Humanoid RoboticsHualong Ren humanoid dexterous hand systems

Rohan Maheshwari navigation/path planning in dynamic environments; bipedal walking

Mishel Johns Task planning; Predictive control algorithms

Chris Dembia human motion synthesis

Medical RoboticsKyuwon Kim exoskeleton or robotic limbs for patients

Shiquan Wang Communication optimization for telemanipulator and the control under time delay

HapticsChris Ploch haptic teleoperation

DesignKevin Tong Human Augmenting Exoskeleton Design for Ease of Use?!?!

Path/Trajectory GenerationMinghan Shen Robot Simultaneous Localization and Mapping (SLAM)

Laura Stelzner path planning in dynamic environments

Paul Chen

Thomas Lipp Minimum time trajectories

Luke Allen Path planning for walking robots

Matt Kiener Task planning for compliant motions (contact situations)

Aerial roboticsMartina Troesch Flying robots without external motion capture or micro flying robots with flapping wings

Hao Jiang Airplane Perching

Margaret Chapman Perception and obstacle avoidance for aerial unmanned vehicles (UAVs)

Controller DesignBrian Soe Adaptive and learning control

Patrick Sherman Nonlinear and adaptive control techniquies

Li Xuesen Adaptive and learning control

Lipeng Alex Liang Adaptive and Feedback control

CS327A Paper Review Schedule (June 6, 2008) AI Lab, 1st floor in Gates

Session 1: Haptics and Robotic Surgery2:00-2:15pm Minimally Invasive Surgery (David Johnson) 

2:15-2:30pm Surgical Robotics: Needle Steering (Reuben Brewer) 

2:30-2:45pm Haptics (Duong Dang) 

2:45-3:00pm BREAK

Session 2: Human Robotics and Motion Planning3:00-3:15pm Humanoid Robotics: Dynamics and Control (Koichi Hikawa) 

3:15-3:30pm Exoskeleton Control (Adam Leper) 

3:30-3:45pm Motion Control: Sensor Integration (Robert Henriksson) 

3:45-4:00pm Motion Planning (Chen Gu) 

4:00-4:15pm BREAK

Session 3: Tactile and Pressure Sensing4:15-4:30pm Pressure Sensitive Skin (John Ulmen) 

4:30-4:45pm Pressure Sensing (Dan Aukes) 

4:45-5:00pm Tactile Sensing (Paul Nangeroni) 

CS327A Symposium

27th May

Lunch: 12.30pm-1pm

6 Sessions: 1pm-5pm Humanoids, Robot Motion Planning,

Robot Cooperation, Evolutionary Robotics, Robot Design, Haptics

17 speakers: 10 minute talk + 4 minute questions

91 92

93 94

95 96

17

1 : Humanoids 1.00-1.15 pm : Ryan Anderson

Developing personal robots, HRP-2

1.15-1.30 pm : Fred Landavazo IV

Human-robot social interaction

1.30-1.45 pm : Shisheng Cui

Humanoid walking algorithms

1.45-2.00 pm : Haizi Yu

Passive dynamic bipedal walking

2.00-2.15 pm : Shruti Gupta, Taylor Newton, Xiao Ge

Asimo sign language demo

2 : Robot Motion Planning

2.15-2.30 pm : Qiao Zhao Real time vision for robots

2.30-2.45 pm : Chintan Hossain Robot localization and path planning

3 : Robot Cooperation

2.45-3.00 pm : Richard Harris Controlling autonomous robots

3.00-3.15 pm : Ken Oslund Controlling multi-robot formations in

real-time

4 : Evolutionary Robotics

3.15-3.30 pm : Bohua Wang Evolving robots with genetic

algorithms

3.30-3.45 pm : Gerald Brantner Genetic fitness functions that help

robots to move

5 : Robot Design

3.45-4.00 pm : Xiyang Yeh Underwater robot actuation

4.00-4.15 pm : Dan Lopez + Cameron Schaeffer

Swarm robotics algorithms

Designing and controlling swarm robots

6 : Haptics

4.15-4.30 pm : Yu-Wei Lin Overview of haptics and virtual reality

4.30-4.45 pm : Xiao Ge Haptics applications in surgical

robotics

4.45-5.00 pm : Zhan Fan Quek Designs for desktop and wearable

haptic devices

97 98

99 100

101 102