4/13/2021

1

CS225A

Experimental Robotics

Lecture 5 

Oussama Khatib

Project ProposalsExperimental Robotics

1

2

4/13/2021

2

Spring2021 Projects

Sports EnvironmentCookingGroceries

Human Robot InteractionMedicalService

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

3

4

4/13/2021

3

Project ProposalsExperimental Robotics

.. motion in contact

Operational Space Framework

5

6

4/13/2021

4

Joint Space Control

Joint Space Control

7

8

4/13/2021

5

Joint Space Control

F

( )GoalV xF

( )GoalV x

T FJ

Task‐Oriented Control

9

10

4/13/2021

6

F

dynamics( )F F

x

x Fp

Task‐Oriented Control

Unified Motion & Force Control

motion contactF F F contactF

motionF

11

12

4/13/2021

7

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

Lagrange Equations in Operational Space

( ) Fd L L

dt x x

L VT

( ) GravityVF

d T T

dt x x x

Inertial forces Gravity vector

( )GravityV V q

Kinetic Energy

Potential Energy (Gravity)

Lagrangian

Since

13

14

4/13/2021

8

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

Operational Space Control

( )TJ q F

F

1

2

T

Goal p Goal GoalV k x x x x

( )GoalV xF

15

16

4/13/2021

9

1

2

T

Goal p Goal GoalV k x x x x

System GravityT V

Fd T

dt x x

ˆGoal GravityF V V

X

Passive Systems

0GoalTd T

dt x x

V Stable

Conservative Forces

Asymptotic Stability

is asymptotically stable if

0 ; 0TsF x for x

0s v vF k x k

ˆp Goal vF k x x k x p Control

s

GoalTd T

dt x xF

V a system

sFx

p̂ Estimate of p

17

18

4/13/2021

10

Artificial Potential Field

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

19

20

4/13/2021

11

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

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

21

22

4/13/2021

12

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

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

23

24

4/13/2021

13

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

25

26

4/13/2021

14

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 sc

m sc m m c

2m2 2m m

20 2 2 2

22 2 2

1 1

1 1

m m s m sc

m sc m m c

27

28

4/13/2021

15

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

29

30

4/13/2021

16

0

*) ( , ) ( )(mI x x dx tGx F

1 ˆ( )G x I

1( , )x x P

( ) :d t unmodeled disturbances

Dynamic Decoupling: Closed Loop

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

31

32

4/13/2021

17

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

33

34

4/13/2021

18

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 1max

dx

1

( ) 1

x if xsat x

sign x if x

35

36

4/13/2021

19

Trajectory Tracking

Trajectory: , , 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

37

38