operational space dynamics-15 - stanford university · i iml m d 22 mm 1 1 x y m1 m2 ... ddd
TRANSCRIPT
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
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 V
X
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
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
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
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
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
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
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 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
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
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
10
Compliant MotionControl Compliant Manipulation Primitives
Advanced Manipulation Capabilities
Multi-contact Manipulation
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
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
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
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
4T
The desired angular acceleration
4T
d d d
Euler Parameters
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
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