Computer Science
A family of rigid body models:
connections between quasistatic and
dynamic multibody systems
Jeff Trinkle
Computer Science Department
Rensselaer Polytechnic Institute
Troy, NY 12180
Jong-Shi Pang, Steve Berard, Guanfeng Liu
Computer Science
Motivation
Dexterous Manipulation
Planning
Parts Feeder Design
Parts feeder design goals:
Valid quasistatic plan exists
1)
Exit orientation
independent of
entering orientation
2)
High throughput
Design geometry of feeder
to guarantee 1) and
maximize 2).
Part enters cg down
Feeder geometry has 12
design parameters
No quasistatic plan found,
but dynamic plan exists
Evaluate feeder design via
simulation
Part enters cg up
Computer Science
Simulation of Pawl Insertion
Computer Science
Past Work in Quasistatic Multibody Systems
Grasping and Walking Machines – late 1970s.
Used quasistatic models with assumed contact states.
Whtney, “Quasistatic Assembly of Compliantly Supported Rigid Parts,”
ASME DSMC, 1982
Caine, Quasistatic Assembly, 1982
Peshkin, Sanderson, Quasistatic Planar Sliding, 1986
Cutkosky, Kao, “Computing and Controlling Compliance in Robot
Hands,” IEEE TRA, 1989
Kao, Cutkosky, “Quasistatic Manipulation with Compliance and
Sliding,” IJRR, 1992
Peshkin, Schimmels, Force-Guided Assembly, 1992
Computer Science
Past Work in Quasistatic Multibody Systems
Mason, Quasistatic Pushing, 1982 - 1996
Brost, Goldberg, Erdmann, Zumel, Lynch, Wang
Trinkle, Hunter, Ram , Farahat, Stiller, Ang, Pang, Lo, Yeap, Han,
Berard, 1991 – present
Trinkle Zeng, “Prediction of Quasistatic Planar Motion of a
Contacted Rigid Body,” IEEE TRA, 1995
Pang, Trinkle, Lo, “A Complementarity Approach to a Quasistatic
Rigid Body Motion Problem,” COAP 1996
Computer Science
Hierarchical Family of Models
• Models range from pure geometric to
dynamic with contact compliance
• Required model “resolution” is dependent
on design or planning task
Model Space
Dynamic
• Approach:
– Plan with low resolution model first
– Use low resolution results to speed
planning with high resolution model
– Repeat until plan/design with required
accuracy is achieved
Quasistatic
Kinematic
Geometric
Rigid
Compliant
Computer Science
Components of a Dynamic Model
Newton-Euler Equation
Defines motion dynamics
Kinematic Constraints
Describe unilateral and bilateral constraints
Normal Complementarity
Prevents penetration and allows contact
separation
Friction Law
Defines friction force behavior:
Bounded magnitude
Maximum Dissipation
Leads to tangential complementarity
Maintains rolling or allows transition
from rolling to sliding
Quasistatic model:
time-scale the
Newton-Euler
equation.
Computer Science
Complementarity Problems
n
z
Let
be an element of  and
n
let w(z ) be a given function in  .
Find z such that:
w( z ) : n  n
0w  z0
Linear Complementarity Problem of size 1.
Given constants
R and b , find z such that:
w
w  Rz  b
0w  z0
z
Computer Science
Newton-Euler Equation
Non-contact
forces
q - configuration
v - generalized velocity
M - symmetric, positive definite
inertia matrix
f
- non-contact generalized forces
G
- Jacobian relating generalized
velocity and time rate of
change of configuration
M (q(t ))v(t )  f (t , q(t ), v(t ))
q (t )  G (q(t ))v(t )
where
dx
x 
dt
Computer Science
Kinematic Quantities at Contacts
Normal and tangential displacement
functions:
 in (t , q (t )) 

 it (t , q (t ))  i  1,..., N
 io (t , q (t )) 
Locally, C-space is represented as:
 in (t , q(t ))  0; i  1,  , N
tˆi
n̂i
 it
 in
q
Computer Science
Normal Complementarity
i 
it
Define the contact force
in
i (t )  [in it io ]
T
Normal Complementarity
0   n (t , q)  n (t )  0
where
 n  [  in ]
T
n  [ in ]
T
tˆi
n̂i
Computer Science
Dry Friction
Assume a maximum dissipation law
(it , io )  arg min(  it (t , q, v)it   io (t , q, v)io )
where
(it , io )  (i in );  i  1,..., N
( it , io ) is the contact slip rate
Linearized Coulomb
Coulomb
Friction
Friction
Friction
Slip
Slip
Slip
Computer Science
Instantaneous-Time Dynamic Model
q  Gv
Mv  f (t , q, v)  Wn n  Wt t  Wo o
0   n (t , q)  n  0
(t , o )  arg min(  tT (t , q, v)t
  oT (t , q, v)o )
(t , o )  ( n )
Non-contact
forces
Computer Science
Scale the Times of the Input Functions
Scale the driving inputs. Replace
t
with
t
in the driving input functions.
q (t )  G (q)v(t )
M (q)v(t )  f (t , q, v)  Wn (t , q)n (t )  Wt (t , q)t (t )  Wo (t , q)o (t )
0   n (t , q)  n (t )  0
(t (t ), o (t ))  arg min(  tT (t , q, v)t (t )   oT (t , q, v)o (t ))
(t , o )  ( n )
Computer Science
Time-Scaled Dynamic Model
Change variables
  t
q~ ( )  q (t )
v~( )   1v(t )
~
 ( )   (t )
Application of chain rule and algebra yields:
~
d
v
~
~
~
2
~
~
 M
 f ( , q , v )  Wn n  Wt t  Wo o
d
~
~
0   n ( , q )  n  0
~ ~
~T d t
~T d o
~
~
(t , o )  arg min( t
( , q , v )  o
( , q~, v~ ))
d
d
~ ~
~
(t , o )  (n )
Computer Science
Time Stepping Methods
Approximate derivatives by: dx / d  ( x l 1  x l ) / h
th
where h is the time step, x l  x ( l ), and  l is the l
scaled time at which the state of the system was obtained.
~l 1
l 1
l
~
~
~
~
 M (v  v )  f ( , q , v )  W
2
 n
~l 1
l 1
~
0    (W v 
)h  n  0

l
n
T
n
~l 1 ~l 1
~l 1 T T ~ l 1  t
~l 1 T T ~ l 1  o
(t , o )  arg min(( t ) (Wt v 
)  (o ) (Wo v 
))


~l 1 ~l 1
~
(t , o )  (n )
q~ l 1  q~ l  Gv~ l 1h
Computer Science
LCP Time-Stepping Problem
6B
2
0



M


  l 1  
T
W
 n l 1    n
 f   Wf T
 l 1  
    0
N
F
Wn
Wf
0
0
0
0
U
 ET
N
0   v~ l 1     2 Mv  hf 
  ~l 1  

0   n   n / h   n /  

~
l

1

E f  
 n / 
  l 1  

0    
0

~
  n l 1 
 nl 1 
 l 1 
 ~l 1 
0   f    f   0
  l 1 
 l 1 




q~ l 1  q~ l  Gv~ l 1h
Constraint
Stabilization
Kinematic
Control
Size  6B  2N  F
Computer Science
Example: Fence and Particle
Assume:
Particle is constrained from below
Non-contact force: f  [0 0  mg]T
Fence is position-controlled
Wall is fixed in place
Expected motion:
Quasistatic: no motion when not in
contact with fence.
Dynamic: if deceleration of paddle is
large, then particle can continue
sliding without fence contact
Computer Science
Time-Scaled Fence and Particle System
Dynamic
Quasistatic
Boundary
Computer Science
Time-Scaled Fence and Particle System
Dynamic
Quasistatic
Computer Science
Cast Model as Convex Optimization Problem
Introduce the friction work rate value function:
~l 1 T ~l 1
~l 1 T ~l 1
l 1 ~l 1 ~l 1
~
 (v , t , o )  (t ) bt (v )  (o ) bo (v )
 t
l 1
T ~ l 1
~
bt (v )  (Wt v 
)

 o
l 1
T ~ l 1
~
bo (v )  (Wo v 
)

Linear in
v~ l 1
Introduce the friction work rate minimum value function:
l 1
l 1 ~l 1 ~l 1
~
~
 (v )  min  (v , t , o )
*
~ ~
~
(t , o )  (n )
Computer Science
Equivalent Convex Optimization Problem
min  f T v~ l 1   * (v~ l 1 )
OPT :=
v~ l 1
l 1
l 1
~
~
s.t. W v  bn (v )
T
n
 * (v~ l 1 ) is convex. Therefore  * (v~ l 1 ) is
~ l 1 ) is convex.
concave and   * (v
Hypograph of
KKT conditions are exactly the discrete-time model.
Computer Science
Theorem
If
l 1 ~ l 1 ~ l 1 ~ l 1
~
(v , n , t , o ) solves the model with quadratic friction
v~ l 1 is a globally optimal solutions of OPT corresponding
~
~ l 1 is a globally optimal solution to OPT for
to nl 1. Conversely, if v
~
~
a given nl 1 and if nl 1 is equal to an optimal KKT multiplier of the
cone, then
~l 1 ~l 1
constraint in OPT, then defining (t , o ) as below, the tuple
l 1 ~ l 1 ~ l 1 ~ l 1
~
(v , n , t , o ) solves the model with quadratic friction cone.
~l 1
~l 1
it   i in
 it
 it2   io2
~
~
iol 1   i inl 1
 io
 it2   io2
Computer Science
Proposition: Solution Uniqueness
l 1 ~ l 1 ~ l 1 ~ l 1
~
(v , n , t , o ) of the
~ l 1 is the unique
discrete-time model with quadratic friction cone, v
Corresponding to the solution
solution of OPT, if and only if the following implication holds:
Added motion does
not decrease work
f T dv~ l 1  0
T ~ l 1
W
 0; i |  in  0, in  0
it dv
Added motion does not
change friction work.
W T dv~ l 1  0; i |   0,   0
io
Added motion does
not cause penetration
in
 dv~ l 1  0
in
WinT dv~ l 1  0; i |  in  0
l 1
where dv~ is a small change in
v~ l 1
Computer Science
Example
Solution is unique with non-zero
quadratic friction on plane
Friction
Slip
Solution is not unique without friction
Solution is not unique with linearized
friction on plane
Friction
Slip
Computer Science
Future Work
Convergence analysis
Experimental validation
Design applications
Computer Science
Fini
Computer Science
Maximum Work Inequalty:
Unilateral Constraints
Friction
Impulse
Linearize the limit curve at contact i :
di4
pifl 1  Di il 1 , il 1  0
di 3 Limit
Curve
di2
di 5
where the columns of Di are the d ij
vectors transformed into C-space.
di6
DiT v l 1 is the vector of the components of
relative velocity at the contact in the d ij
directions.
d i1
di7
di8 Relative
Velocity
0  il 1  DiT vl 1  eli 1  0
0  li 1  i pinl 1  eT il 1  0
Maximum Work
Boundary or Interior
where
e  [1  1]T
Computer Science
Tangential Complementarity: Example
0
0 
 ( D v )1    0

T l 1
l 1
 ( Di v )8  i  0
l 1
1
T
i
l 1
8
l 1
Friction
Impulse
l 1
i
di4

l 1
 ( D v ) 8
T
i
 8l 1   i pinl 1

l 1
j
 0,  j  8
Curve
di2
di 5
di6
l 1
i
di 3 Limit
d i1
di7
di8 Relative
Velocity
Computer Science
Instantaneous Rigid Body Dynamics in the Plane
anR  ( Ann ) R R
 a  ( A )
 nR    nn RR
 atR   ( Atn ) RR
   
 stR   0
( Ann ) R R
( Ant ) R R
( Ann ) RR
( Ant ) RR
( Atn ) RR
( Att ) RR
2U R
I
 a nR   c nR 
 a  c 
   nR   0
0   nR


 atR
  stR

    
 stR   atR 
UR
0 cnR  bnR 
0  cnR  bnR 
 

I   stR   btR 
   
0  atR   0 
Size  N R  3N R
- diagonal matrix of friction coefficients at rolling contacts
Computer Science
Example: Sphere initially translating on
horizontal plane.
Computer Science
Simulation with Unilateral and Bilateral
Constraints
Computer Science
Time-Stepping with Unilateral Constraints
With Constraint
Without Constraint
Stabilization
Stabilization
q l 3
q l 3
ql 2
Admissible
Configurations
ql
q l 1
Solution always exists and
Lemke’s algorithm can compute
one (Anitescu and Potra).
ql 2
Admissible
Configurations
ql
q l 1
Current implementation uses
stabilization and the “path”
algorithm (Munson and Ferris).
Solution
Non-uniqueness:
Computer Science
LCP Non-Convexity
an  Rc n  b
0  an  cn  0
g ext
l 
b   sin(  ) 
2
m
1 l cos( )
R 
(cos( )   sin(  ))
m
4J
2
an
Two
Solutions
cn
Computer Science
Solution Non-Uniqueness:
Contact Force Null Space
Both friction cones can “see” the other
contact point.
Assume:
Blue discs are fixed in space
Red disc is initially at rest
Solution 1 – disc remains at rest
Solution 2 – disc accelerates downward
External Load