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

Valid quasistatic plan exists

No quasistatic plan found, but dynamic plan exists

Dexterous Manipulation Planning

Part enters cg down

Part enters cg up

Parts Feeder Design

Parts feeder design goals:

1) Exit orientation independent of entering orientation

2) High throughput

Design geometry of feeder to guarantee 1) and maximize 2).

Feeder geometry has 12 design parameters

Evaluate feeder design via simulation

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 - 1996Brost, 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

• 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

Model Space

Rigid Compliant

Dynamic

Quasistatic

Geometric

Kinematic

Computer Science

Components of a Dynamic Model

Newton-Euler EquationDefines motion dynamics

Kinematic ConstraintsDescribe unilateral and bilateral constraints

Normal ComplementarityPrevents penetration and allows contact separation

Friction LawDefines friction force behavior:

Bounded magnitudeMaximum Dissipation

Leads to tangential complementarity

Maintains rolling or allows transition

from rolling to sliding

Quasistatic model: time-scale the Newton-Euler equation.

Computer Science

Let be an element of and

let be a given function in . Find such that:

Complementarity Problems

nz)(zw

w0 0zzn

bRzw w0 0z

Linear Complementarity Problem of size 1.

Given constants and , find such that:R b zw

z

nnzw :)(

Computer Science

Newton-Euler EquationNon-contact

forces- configurationq

- generalized velocityv

- symmetric, positive definite inertia matrix

M

- non-contact generalized forces

f

))(),(,()())(( tvtqtftvtqM

)())(()( tvtqGtq

- Jacobian relating generalized velocity and time rate of change of configuration

G

dt

dxx where

Computer Science

Kinematic Quantities at Contacts

Ni

tqt

tqt

tqt

io

it

in

,...,1

))(,(

))(,(

))(,(

Locally, C-space is represented as:

;0))(,( tqtin Ni ,,1

q

it̂ in̂

init

Normal and tangential displacement functions:

Computer Science

Normal Complementarity

Tioitini t ][)(

it

0)(),(0 tqt nn

whereT

inn ][ T

inn ][

Define the contact force

Normal Complementarity

in̂it̂

i

in

Computer Science

Dry Friction

Friction

Slip

Coulomb

),( ioit

Assume a maximum dissipation law

)),,(),,(min(arg),( ioioititioit vqtvqt

Ni ,...,1);(),( iniioit where

is the contact slip rate

Slip

Friction

Linearized Coulomb

Slip

Friction

Computer Science

Instantaneous-Time Dynamic Model

tTtot vqt ),,(min(arg),(

)),,( oTo vqt

Non-contact forces

oottnn WWWvqtfvM ),,(

0),(0 nn qt

Gvq

)(),( not

Computer Science

Scale the Times of the Input Functions

)(),( not

)()()( tvqGtq

Scale the driving inputs. Replace with in the driving input functions.

))(),,()(),,(min(arg))(),(( tvqttvqttt oTot

Ttot

0)(),(0 tqt nn

)(),()(),()(),(),,()()( tqtWtqtWtqtWvqtftvqM oottnn

t t

Computer Science

)~

()~

,~

( not

oottnn WWWvqfd

vdM

~~~

)~,~,(~

2

0~

)~,(0 nn q

)()(~ tqq Change variables

t

Time-Scaled Dynamic Model

)()(~

t )()(~ 1 tvv

Application of chain rule and algebra yields:

))~,~,(~

)~,~,(~

min(arg)~

,~

( vqd

dvq

d

d oTo

tTtot

Computer Science

Approximate derivatives by:

where is the time step, , and is the

scaled time at which the state of the system was obtained.

Time Stepping Methods

hxxddx ll /)(/ 1 h )( l

l xx lthl

hvGqq lll 11 ~~~

)~

()~

,~

( 11n

lo

lt

112 ~)~,~,()~~( lll WvqfvvM

0~

)~(0 11

ln

nlTn

ln hvW

))~()~

()~()~

min((arg)~

,~

( 111111

olTo

Tlo

tlTt

Tlt

lo

lt vWvW

Computer Science

LCP Time-Stepping Problem

0

/

//~

~~

00

00

000

00 2

1

1

1

12

1

1

1

n

nn

l

lf

ln

l

T

Tf

Tn

fn

l

lf

ln h

hfMvv

EU

EW

W

WWM

0~

~

01

1

1

1

1

1

l

lf

ln

l

lf

ln

hvGqq lll 11 ~~~

Constraint Stabilization Kinematic

Control

N NFB6

FNBSize 26

Computer Science

Assume:

Particle is constrained from below

Non-contact force:

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

Example: Fence and Particle

Tmgf ]00[

Computer Science

Time-Scaled Fence and Particle System

Dynamic

QuasistaticBoundary

Computer Science

Time-Scaled Fence and Particle System

Dynamic

Quasistatic

Computer Science

Introduce the friction work rate value function:

Cast Model as Convex Optimization Problem

)~

()~

,~

( not

)~()~

()~()~

()~

,~

,~( 1111111 lo

Tlo

lt

Tlt

lo

lt

l vbvbv

)~()~( 11

tlTt

lt vWvb

)~

,~

,~(min)~( 1111* lo

lt

ll vv

)~()~( 11

olTo

lo vWvb

Linear in1~ lv

Introduce the friction work rate minimum value function:

Computer Science

Hypograph of is convex. Therefore is

concave and is convex.

KKT conditions are exactly the discrete-time model.

Equivalent Convex Optimization Problem

)~(~min 1*1 llT vvf 1~ lv

)~( 1* lv

)~(~.. 11 ln

lTn vbvWts

OPT :=

)~( 1* lv)~( 1* lv

Computer Science

If solves the model with quadratic friction

cone, then is a globally optimal solutions of OPT corresponding

to . Conversely, if is a globally optimal solution to OPT for

a given and if is equal to an optimal KKT multiplier of the

constraint in OPT, then defining as below, the tuple

Theorem

)~

,~

,~

,~( 1111 lo

lt

ln

lv 1~ lv

1~ ln

)~

,~

( 11 lo

lt

1~ ln

1~ lv1~ l

n

)~

,~

,~

,~( 1111 lo

lt

ln

lv solves the model with quadratic friction cone.

22

11 ~~

ioit

itlini

lit

22

11 ~~

ioit

iolini

lio

Computer Science

where is a small change in

Corresponding to the solution of the

discrete-time model with quadratic friction cone, is the unique

solution of OPT, if and only if the following implication holds:

Proposition: Solution Uniqueness

1~ lv)

~,

~,

~,~( 1111 l

ol

tl

nlv

0~ 1 lvd

0|;0~ 1 in

lTin ivdW

0~ 1 lT vdfAdded motion does not decrease work

0,0|;0~ 1 inin

lTit ivdW

0,0|;0~ 1 inin

lTio ivdW

Added motion does not change friction work.

Added motion does not cause penetration

1~ lvd 1~ lv

Computer Science

Example

SlipFriction

Solution is unique with non-zero quadratic friction on plane

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

where the columns of are the vectors transformed into C-space.

is the vector of the components of relative velocity at the contact in the directions.

Maximum Work Inequalty: Unilateral Constraints

1lTi vD

Linearize the limit curve at contact

Friction Impulse

Relative Velocity

3id

1id

2id

4id

5id

6id

7id8id

Limit Curve0, 111 l

ilii

lif Dp

ijd

iD

00 111 li

Tlini

li ep

Boundary or Interior

00 111 li

lTi

li evD Maximum Work

Te ]11[ where

:i

ijd

Computer Science

Tangential Complementarity: Example

0)(0 11

111 l

ilT

il vD

8,01 jlj

0)(0 18

118 l

ilT

il vD

118

lini

l p

Friction Impulse

Relative Velocity

3id

1id

2id

4id

5id

6id

7id8id

Limit Curve

811 )( lT

ili vD

Computer Science

0020

)()()(

0)()()(

0)()()(

tR

nR

Rn

tR

tR

nR

Rn

R

RRttRRtnRRtn

RRntRRnnRRnn

RRntRRnnRRnn

tR

tR

nR

Rn

b

b

b

a

s

c

c

IU

IAAA

AAA

AAA

s

a

a

a

Instantaneous Rigid Body Dynamics in the Plane

00

tR

tR

nR

Rn

tR

tR

nR

Rn

a

s

c

c

s

a

a

a

RU - diagonal matrix of friction coefficients at rolling contacts

RR NNSize 3

Computer Science

Example: Sphere initially translating on horizontal plane.

Computer Science

Simulation with Unilateral and Bilateral Constraints

Computer Science

Time-Stepping with Unilateral Constraints

Solution always exists and Lemke’s algorithm can compute

one (Anitescu and Potra).

Admissible Configurations

lq

1lq

2lq

3lq

Without Constraint Stabilization

Admissible Configurations

lq

1lq

2lq

3lq

With Constraint Stabilization

Current implementation uses stabilization and the “path”

algorithm (Munson and Ferris).

Computer Science Solution Non-uniqueness:LCP Non-Convexity

bRca nn 00 nn ca

))sin()(cos(4

)cos(1 2

J

l

mR

m

glb ext )sin(

2

na

nc

Two Solutions

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