1

CS225A

Experimental Robotics

Lecture 4

Oussama Khatib

Summary Kinematics

1

2

2

Joint to Operational Space Relationships

𝐽 𝑞 𝐸 𝑥 𝐽 𝑞

𝐽# 𝑞 𝐽# 𝑞 𝐸 𝑥𝛿𝑞 𝐽# 𝑞

𝛿𝑥𝛿𝜙

𝛿𝑥𝛿𝜙 𝐽 𝑞 𝛿𝑞𝛿𝑥 𝐸 𝑥

𝛿𝑥𝛿𝜙

Task Representation 

Forward Relationships

Inverse Relationships 𝛿𝑥𝛿𝜙 𝐸 𝑥 𝛿𝑥

𝑥 Operational point 𝐽 𝑞 Representation specific task Jacobian

𝑥 Op. point orientation in task space 𝐽 𝑞 Basic (rep. ind.) task Jacobian

𝛿𝑥 Change in op. point config.  𝐽# 𝑞 Generalized Inverse of  𝐽

𝛿𝑥 Change in cartesian point position 𝐽# 𝑞 Generalized Inverse of  𝐽

𝛿𝜙 Instantaneous angular error 𝐸 𝑥 Transforms Basic Jacobian to Task Specific Jacobian

𝛿𝑞 Change in joint value 𝐸 𝑥 Inverse of the 𝐸 matrix 

( )v

x E x

0

vJ q

P

R

xx

x

Position Representations

Representation 𝑬𝑷 Matrix 𝑬𝑷𝟏 Matrix

Cartesian 𝒙,𝒚, 𝒛1 0 00 1 00 0 1

1 0 00 1 00 0 1

Cylindrical 𝝆,𝜽, 𝒛

𝑐𝑜𝑠 𝜃 𝑠𝑖𝑛 𝜃 0𝑠𝑖𝑛 𝜃

𝜌𝑐𝑜𝑠 𝜃

𝜌 00 0 1

𝑐𝑜𝑠 𝜃 𝜌 𝑠𝑖𝑛 𝜃 0𝑠𝑖𝑛 𝜃 𝜌 𝑐𝑜𝑠 𝜃 0

0 0 1

Spherical 𝝆,𝜽,𝝓

𝑐𝑜𝑠 𝜃 𝑠𝑖𝑛 𝜙 𝑠𝑖𝑛 𝜃 𝑠𝑖𝑛 𝜙 cos 𝜙sin 𝜃

𝜌sin 𝜙𝑐𝑜𝑠 𝜃

𝜌sin 𝜙 0

𝑐𝑜𝑠 𝜃 𝑐𝑜𝑠 𝜙𝜌

𝑠𝑖𝑛 𝜃 𝑐𝑜𝑠 𝜙𝜌

𝑠𝑖𝑛 𝜙𝜌

𝑐𝑜𝑠 𝜃 𝑠𝑖𝑛 𝜙 𝜌 𝑠𝑖𝑛 𝜃 𝑠𝑖𝑛 𝜙 𝜌 𝑐𝑜𝑠 𝜃 𝑐𝑜𝑠 𝜙𝑠𝑖𝑛 𝜃 𝑠𝑖𝑛 𝜙 𝜌 𝑐𝑜𝑠 𝜃 𝑠𝑖𝑛 𝜙 𝜌 𝑠𝑖𝑛 𝜃 𝑐𝑜𝑠 𝜙𝑐𝑜𝑠 𝜙 0 1 𝜌 𝑠𝑖𝑛 𝜙

3

4

3

Orientation Representations

Rep. 𝑬𝑹 Matrix 𝑬𝑹𝟏 Matrix 𝜹𝝓 𝑬𝑹 𝒙𝑹 𝜹𝒙𝑹

Direction Cosines

𝒙𝒓 𝒓𝟏𝑻, 𝒓𝟐

𝑻, 𝒓𝟑𝑻 𝑻

�̂��̂��̂�

�̂��̂��̂�

12�̂� 𝑟 �̂� 𝑟 �̂� 𝑟

Euler Angles𝒙𝒓 𝝍,𝜽,𝝓

𝑠𝑖𝑛 𝜓 𝑐𝑜𝑠 𝜃𝑠𝑖𝑛 𝜃

𝑐𝑜𝑠 𝜓 𝑐𝑜𝑠 𝜃𝑠𝑖𝑛 𝜃 1

𝑐𝑜𝑠 𝜓 𝑠𝑖𝑛 𝜓 0𝑠𝑖𝑛 𝜓

𝑠𝑖𝑛 𝜃𝑠𝑖𝑛 𝜓

𝑠𝑖𝑛 𝜃 0

0 𝑐𝑜𝑠 𝜓 𝑠𝑖𝑛 𝜓 𝑠𝑖𝑛 𝜃0 𝑠𝑖𝑛 𝜓 𝑐𝑜𝑠 𝜓 𝑠𝑖𝑛 𝜃1 0 𝑐𝑜𝑠 𝜃

0 𝑐𝑜𝑠 𝜓 𝑠𝑖𝑛 𝜓 𝑠𝑖𝑛 𝜃0 𝑠𝑖𝑛 𝜓 𝑐𝑜𝑠 𝜓 𝑠𝑖𝑛 𝜃1 0 𝑐𝑜𝑠 𝜃

𝜓𝜃𝜙

EulerParameters

𝝀 𝝀𝟎,𝝀𝟏,𝝀𝟐,𝝀𝟑

12

𝜆 𝜆 𝜆𝜆 𝜆 𝜆𝜆 𝜆 𝜆𝜆 𝜆 𝜆

2 𝜆 𝜆 𝜆 𝜆𝜆 𝜆 𝜆 𝜆𝜆 𝜆 𝜆 𝜆

2 𝜆 𝜆 𝜆 𝜆𝜆 𝜆 𝜆 𝜆𝜆 𝜆 𝜆 𝜆

𝜆𝜆𝜆 𝜆

Kinematics

Dynamics

Jacobians

Inverses

Task

Representations

Equations of Motion

Operational Space Control

Dynamic

Models

Compliance

Force Control

Control

Modalities

Redundant

Robots

Posture

Null Space

Dynamic Behavior

Whole-Body Control

Menu

5

6

4

OperationalSpace

Framework!

Simulation of StanBotServing a Volleyball

7

8

5

Torque‐Controlled Robots

KUKA ‐ iiwaKUKA ‐ iiwa PANDAPANDA

9

10

6

SAI 2.0: Physical Simulation & Control

SAI 2.0: Physical Simulation & Control

11

12

7

Hands Underwateron Landin the Air

13

14

8

15

16

9

CS225A – Spring Projects

17

18

10

Final Project PresentationsExperimental Robotics (CS225A)Tuesday, November 17, 2020 

19

20

11

TennisBot Gordon GroceryBot

William KoskiDimitri PetrakisChenkai

Ewurama KarikariZhengqiu LouJoseph WangJiaqiao Zhang

Gabriela Bravo IllanesMax FarrChun Ming Zhang

RehaBot DocBot DrawBot

Eleonore JacquemetRuta JoshiJuhi Madan

Yuxiao ChenDan FanShivani Guptasarma

Kaojun HeAce HuRan Le

CS225A – Spring2021 Projects

Sports EnvironmentCookingGroceries

Human Robot InteractionMedical

21

22

12

.. motion in contact

Whole‐body Compliance!

Joint Space Control

23

24

13

Joint Space Control

Joint Space Control

25

26

14

F

( )GoalV xF

( )GoalV x

T FJ

Task‐Oriented Control

F

dynamics( )F F

x

x Fp

Task‐Oriented Control

27

28

15

Unified Motion & Force Control

motion contactF F F contactF

motionF

Equations of Motion

d L LF

dt x x

with ( , ) ( , ) ( )gravityL x x T x x V x

29

30

16

End‐Effector Control

( )TJ q F F

1

2

T

goal p g gV k x x x x

goalVF

1

2

T

goal p g gV k x x x x

System gravityT Vd T

Fdt x x

ˆ

goal gravityF V VX

Passive Systems

0

goalT Vd T

dt x x

StableConservative Forces

31

32

17

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

Artificial Potential Field

33

34

18

( ) ( , ) ( )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

Example: 2‐d.o.f arm

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

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

1q1l

2q2l

35

36

19

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

37

38

20

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

Equations of Motionvcii

Pci

Link i

TTotal Kinetic Energy:

1

2Lin i

T

kAqT T q

39

40

21

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

41

42

22

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

iiCJ 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 Motion

vcii

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

Equations of Motion

43

44

23

( )

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

Christoffel Symbols 1( )

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

45

46

24

Gravity Vector

m2g

c2

m1g

c1mng

cnm3g

c3

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

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

47