Computer Science

A family of rigid body models: connections between quasistatic and

dynamic multibody systems

Jeff TrinkleComputer Science DepartmentRensselaer 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, 1982Peshkin, Sanderson, Quasistatic Planar Sliding, 1986Cutkosky, Kao, “Computing and Controlling Compliance in Robot

Hands,” IEEE TRA, 1989Kao, Cutkosky, “Quasistatic Manipulation with Compliance and

Sliding,” IJRR, 1992Peshkin, 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 complementarityMaintains rolling or allows

transition from rolling to sliding

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

Computer Science

Let be an element of andlet be a given function in . Find such that:

Complementarity Problemsnz

)(zww0 0zz

n

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

dtdxx where

Computer Science

Kinematic Quantities at Contacts

Nitqttqttqt

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

where Tinn ][

Tinn ][

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 WWWvqfdvd

M

~~~)~,~,(~

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

dd oT

otT

tot

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/

//~~~

0000

00000 2

1

1

1

12

1

1

1

n

nn

l

lf

ln

l

T

Tf

Tn

fn

l

lf

ln h

hfMvv

EUEW

WWWM

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 in 1~ 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 Interior00 111 l

ilT

ili evD

Maximum WorkTe ]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

bbb

ascc

IUIAAA

AAAAAA

saaa

Instantaneous Rigid Body Dynamics in the Plane

00

tR

tR

nR

Rn

tR

tR

nR

Rn

ascc

saaa

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

lq1lq

2lq

3lq

Without Constraint Stabilization

Admissible Configurations

lq1lq

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

Jl

mR

mglb ext )sin(

2

na

nc

Two Solutions

Computer ScienceSolution Non-Uniqueness:Contact Force Null Space

Both friction cones can “see” the other contact point.

Assume:Blue discs are fixed in spaceRed disc is initially at rest

Solution 1 – disc remains at restSolution 2 – disc accelerates downward

External Load