USER INTERACTION WITH CAD MODELS WITH NONHOLONOMIC PARAMETRICSURFACE CONSTRAINTS

Donald D. Nelson Elaine CohenUniversity of Utah

Computer Science50 S Central Campus Dr Rm 3190

Salt Lake City, UT 84112-9205Email:

�dnelson,cohen � @cs.utah.edu

ABSTRACTUsermanipulationof assemblymodelscanprovide insight

during the early, formulative designstagesinto kinematicanddynamiccharacteristicsof a mechanism.We presenttheadvan-tagesof kinematicrepresentationof constraintequationsin fullyCartesiancoordinates,a departurefrom standardpracticefor in-teractivemechanicalassemblyat interactiverates.

Formulationsof asurfacerolling contactconstraintequationandits Jacobian,definedasajoint betweentwoNURBSsurfacesvia position,tangency andvelocity constraintrelations,arede-rivedfor usein dynamicsimulationandassemblyoptimization.The constraintequationformulationsusequaternionsto repre-sentorientation. An appendixdevelopsappropriatedifferentialalgebra.

In this work we develop the use of constraintsin globalframeCartesiancoordinatesfor describingoperator-in-the-loopinteractionswith mechanicalassembliesundera unified frame-work combininglower-pair jointsandmoregeneralsurfacecon-tactinteractions.

1 IntroductionThekinematicsanddynamicsof thecomplicated,highlyun-

constrained,jointedmechanismsthatarisein virtual prototypingaredifficult to analyze.Frequentlyoccurringexamplesincludesmoothfingeredgrasp,spatialcam-follower mechanisms,andbeltdrives.

Constraintequationformulationscan be used to supportsuchunusualjoints in kinematicoptimizationproblemsanddy-namics. It is thennecessaryto decidethe form, or generalizedcoordinateswith which to expresstheseequations.Generalizedcoordinatesareany setof coordinatesusedto describesystem

configuration;further, any quantitymayserveasgeneralizedco-ordinates[Goldstein,1980],[Shabana,1994].Theequationsap-plicableto theabovecitedexamplescanbeexpressedin absoluteCartesiangeneralized coordinates, (ACGC) [Shabana,1994].Reduced(joint-space)generalized coordinatesmay be usedasan alternative, but this alternative is not applicableto jointswith many degreesof freedomor jointswith nonholonomiccon-straintsthatcannotbeparameterized[Baraff, 1996].

TheNewton-Eulerequationsandotherwell known dynam-ics formulationscan be expressedin ACGC, and augmentedwith Lagrangemultipliers to accountfor joint constraintforces[Haug,1992],[Shabana,1994]. We have foundthis approachtohave desirablecharacteristicsin software engineeringand ex-pressivenessdueto modularassociationof equationswith bod-ies. Also, it is known to have linearcostin thenumberof con-straints[Baraff, 1996]. This techniqueusessparseconstraintJacobianmatrices,that is, partial derivativesof geometriccon-straintexpressionsthatenforcetherequirementthatjointsremainconnected.

Lagrangemultiplierscanbeusedwith theparameterizationof complicatedjointswhenthejoint coordinatesareACGC.Forexample,thenonholonomicsurfacerolling contact“joint” asinFig. 1 canbeusedto handlevelocity-dependentconstraintsin aframeworkwhichmanagesdifferentialoperationsonconstraints.

Thesurfacecontactexampleshown in Fig.2 is aninterestingcasethatcancomeupin kinematicanddynamicanalysisof kneeor spatialcammechanisms.Thebeltdrivein Fig.3 is anexampleof aplanarcurvecontact.Theanalysesin Sections4, 5,and6 areapplicableto bothplanarcurveandspatialsurfaceconstraints.

Surfacejoints will have threepositiondegreesof freedom(Fig. 4). While eachbodyhassix degreesof freedomin veloc-ity, threeof theseareremovedin therolling contactconstraints.

daP

Figure 1. A wheeled vehicle can reach any position and orientation; its

position is not constrained. The constraint that can be written is nonholo-

nomic: the velocity of P along the axle is zero: a � d � 0 [Haug, 1992].

Figure 2. Two links in surface-surface contact, held together.

Figure 3. Two links under curve contact constraint. At right is a belt drive

transformed into a rolling contact mechanism [Erdman, 1993].

Computationaltechniquesfor surfacederivativesarepresentedin Section5.

S2@time 2

twist about normal

S2@time 1

u direction rollv directionroll

S1

Figure 4. 3 position degrees of freedom are available in a surface con-

tact joint.

2 Background on Surface ContactMany important contactproblemsare classifiedas either

unilateralor bilateralconstraints.A bilateralconstraintexpressesanequationof equality. Theforceof constraintin thiscasecanbein eitherdirectionof thesurfacenormal. A unilateralconstraintexpressesinequalityand hasconstraintforce in one direction,along the outward surfacenormal. This work is addressedto-wardsunilateralconstraintsfor dynamicsandbothunilateralandbilateralconstraintsfor assemblyoptimization.

Otherwork hastreatedsurface-surfacecontacts,or “againstsurfaces”in termsof aunilaterallinearcomplementarityproblemor asa quadraticprogrammingproblem[Lotstedt,1982]. A di-rectmethodwithoutoptimizationis givenin [Baraff, 1994].Thenatureof thedynamicsproblemtreatedherediffersin thatweareconcernedwith thegeometryof smoothsurfacejoints modeledas bilateralconstraintswhich hold together, ratherthanunilat-eral constraintsandfriction forcesof flat or polygonalsurfacecontacts.

Work on rolling contacthasstudiedtheproblemsof grasp-ing, pushing,and steering. Prior work in this areabeginningwith [Montana,1988] and [Cai, 1987] has madeuse of time-dependentrelations[Sarkar, 1994], [Yun,1995], [Canny, 1990],[Cremer, 1996], [Jia,1998],[Han,1998]. Theevolutionarystyleof updatingthe point andangleof contactby addingthe para-metric contactcoordinatesto thegeneralizedcoordinatesis notconveniently used with the Cartesiancoordinatesin the La-grangemultiplier constraints,formulated in [Shabana,1994],[Haug,1992], [Baraff, 1996] and this work. We areconcernedwith the developmentof surfacecontactkinematicsin coordi-natesof thepositionandorientationof eachlinkage.

CommercialCAD packagesare beginning to incorporatenew featuresin geometricconstraintssuchas edge-to-surfaceconstraints[ProE], [Adams], andmodelingof mechanismswithsurfaceconstraintsis becomingmoreadvanced.However, anal-ysis of suchassembliesis very applicationspecificandheavilydependenton particularpropertiessuchasplanarcharacteristicsof theconstraintor motion.

3 Linkage SurfacesThetensorproductnon-uniformrationalB-splinedefinition

of a surfaceS is a mappingfrom R2 � R3, i.e. a functionfromparametric(u,v) spaceto Cartesian(x,y,z) space.

S � u � v��� ∑i j wi jpi jBi ku � u� Bj kv � v�∑i j wi jBi ku � u� Bj kv � v�

wherethe B-splineblendingfunctionsB, control meshp, andweightsw areused.

We denotegeneralizedcoordinatesqi to be in Cartesian,world space,for eachsurfacei. Supposeit is necessaryto rotate

2

aNURBSlinkageby aunit lengthquaternion4-tuplecontainingcomponentsof Eulerparametersq4 7 andtranslationby q1 3.

Using the original control mesh,porig, createthe controlpoints of the NURBS surfacein the global frame, pi j . Thenpi j � R � q4 7 � porig

i j � q1 3, whereR is a rotationaloperatorwithquaternionarguments.Thus,pi j asafunctionof q, andS is nowa functionof u � v andp0 0, p0 1, ... pn m, and

S � u � v� p0 0 � q ��� p0 1 � q ���� � � ���� ∑i j wi jpi jBi ju � u� Bj vv � v�∑m

i j wi jBi ku � u� Bj kv � v� (1)

4 Constraint EquationsThis paperpresentsa modularframework in which simple

joints andsurfacecontactjoints canbeusedtogetherfor hapticsassemblyanddynamicinteraction. The surfaceconstraintsarewritten in the mannerof lower-pair geometricjoint constraintsas found in [Yang,1995], [Shabana,1994], [Haug,1992]. Forexample,a sphericaljoint in Fig. 5 indicatesthat the endpointsof thetwo bodiesi and j mustmeet.

Csph � q ��� R � qi4 7 � ki � qi

1 3 � R � q j4 7 � k j � q j

1 3 (2)

Theconstantk in Eqn.2 is alocalbodyframevectorindicat-ing the locationof thesphericaljoint (like porig). The Jacobianof R � qi 4 7 � areusedin Section7 andSection11.

Figure 5. Two differently detailed views of a spatial 1 DOF, 4 bar mech-

anism with two revolute joints, a spherical joint, and a universal joint. The

middle of the middle link is grasped by point contact.

Thesurfaceconstraintequationsexpressthefactthatthesur-facesmusttouch,mustbetangentat thatpoint,and(optionally)mustroll, not slidearound.Thepositionconstraintis

Cp ����� S1 � u1 � v1 � � S2 � u2 � v2 ������� 2 � 0 (3)

where(u1 � v1) and(u2 � v2) areobtainedfromasurface-surfaceop-timizationalgorithm[Kriezis, 1992], [Johnson,1998]. The twopairs

� � u1 � v1 ����� u2 � v2 ��� will be the parametriccontactcoordi-nates.

Tangency constraintsgoalongwith constraintCp to enforcethatthetangentplaneof bothsurfacesdefinethesameplane.

Ctan � �S2

u � u2 � v2 ����� S1u � u1 � v1 ��� S1

v � u1 � v1 ���S2

v � u2 � v2 ����� S1u � u1 � v1 ��� S1

v � u1 � v1 ����� � �00 � (4)

EquationsCp andCtan aresufficient for a sliding NURBS-NURBS contactcondition. In addition,we canwrite a rollingjoint contactconstraint.The rolling contactconstraintrequiresthat no slippageoccurbetweentwo surfaces.To prevent drag-ging or slipping, the amountof movementof the contactpointalong one surfacemust be equal to the amountof movementon the othersurface. Thus,the relative surfacevelocity is con-strainedto reflectthis fact. We write the rolling constraintasatimederivative,Croll , for consistentnotationin Section8.

Wemeasuretherelativechangein surfaceCartesiancontactcoordinatesasaresultof thechangein q1 andq2 with respecttoeachbody’s local frame,or originalsurface,SL. For comparison,therelativecontactvelocity on SL is orientedin theworld frameandprojectedontothecontactplaneatS1 containingvectorsS1

u,S1

v.

S1u � R1 � q1 � S1

L � S1u � R2 � q2 � S2

L

S1v � R1 � q1 � S1

L � S1v � R2 � q2 � S2

L

(5)

Heretheparametriccontactcoordinates� u1 � v1 � and � u2 � v2 �arealsovariableover time,andarefunctionsof thepositionco-ordinatesq1 andq2 of eachbody. The constraintequationthatsatisfiesEqn.5, with thesubstitutionS � RSL � RSL � q1 3, is

Croll � �S1

u ��� S1 � RS1L � q1

1 3 � S2 � RS2L � q2

1 3 �S1

v ��� S1 � RS1L � q1

1 3 � S2 � RS2L � q2

1 3 � � � 02x1

(6)EachtermS reducesto ∂S

∂q q.For the“pure” rolling case,slippageabouttheaxisperpen-

dicularto eitherequivalentsurfacecontactnormalis (optionally)not allowedandremovesa third degreeof freedomfrom theve-locity characteristicsof eachbody. � S1

u � S1v � ��� ω1 � ω2 ��� 0 may

beaddedasa third equationfor Croll , whereω1 andω2 aretheangularvelocitiesof eachbody.

3

5 Surface Evaluation Tools: ∂S∂q

To formulatethegeometricconstraintJacobians,wemustbeableto evaluatethesurfaceJacobian∂S

∂q usein theformulationofthegeometricconstraintJacobians.

Becausean interactive rateapproximateminimumdistancealgorithmis available[Kriezis, 1992], [Johnson,1998] and theanalyticalevaluationof ∂S

∂q is prohibitive, we usenumericaldif-ferentiationfor finding the Jacobianof the minimal distanceequationbetweentwo surfaces.For eachcolumn i in the Jaco-biancorrespondingto a variablein q1 2

i �"! q1Ti q2T

i # T, two sam-plesof the distanceequationareevaluatedand the gradientisapproximatedby centralfinite differences.S1 � u1 � v1 � q1 2 � willbewrittenasS1 � q1 2 � because� u1 � v1 � is obtainedfrom thelocaldistancealgorithmandis dependentonq1 2.

∂S1

∂q1$ 2i

� S1 % q1 $ 2i & h')( S1 % q1$ 2

i ( h'2h

∂S2

∂q1$ 2i

� S2 % q1 $ 2i & h')( S2 % q1$ 2

i ( h'2h

(7)

Eachcolumn of the Jacobianfor eachsurfacerequires2evaluationsof the minimal distanceequation[Kriezis, 1992],[Johnson,1998] for each componentof the translationsandquaternionsin q1 2. This method has error on the order

O(h2), or h2 ∂2S % σ '% ∂q ' 2 , for someσ in the range � q1 2 � h � q1 2 � h�[Cheney, 1985]. One applicationof Richardsonextrapolationwould give an error of O(h4), but at the costof 4 minimal dis-tanceevaluations.Somesimplenumericalanalysisshows thatavery smallvalueof h relative to thecoordinatesmagnitudemaybeselectedin practicewithout resultingin lossof precisiondueto subtractionof very similar numbers.For thepurposesof as-semblyoptimization,theO(h) formula[Hansen,1995],

∂S

∂q1 2i

� S � q1 2i � h� � S � q1 2

i �h

hasoccasionallybeeninadequatefor optimizationpurposes.TheO(h2) methodis usedwhenthe O(h) techniquecausesnonsen-sical results. Accuracy obtainedwith 4 sampleshasnot beenrequiredin examplesencounteredsofar. An additionalcompli-cationoccurssincethequaternioncomponentsof q by h violatesthe unit length constrainton the quaternion. Renormalizationschemes,removal of one of the dependentquaternioncompo-nents,and the useof Euler anglesarepossiblewaysto handlechangein rotationcoordinates.

The“crosstalk” betweenthesurfacesbecomesevidenthereasthepartialderivative is with respectto bothq1 andq2. Moreintuitively, the motion of one surface will changethe closestpoint on itself andtheothersurfaceaswell. We notethatother

polygonalor surfacerepresentationsthathave fastminimal dis-tanceevaluationalgorithmsmayusethefinite differencesalgo-rithm, andthat thesurfaceanalysisin this paperappliesaslongasreasonablysmoothsurfacecharacteristicsareapartof therep-resentation.

6 Constraint JacobianCritical elementsneededin performing kinematicsopti-

mizationanddynamicanalysisaretheconstraintequationsandtheirJacobians.Wederivethepartialderivativesof Cp, Ctan, andCroll , theconstraintequationsof ourapproach.

ThepositionconstraintJacobianfor Cp is

∂Cp

∂ � q1 � q2 � � ∂ ����� S1 � u1 � v1 � � S2 � u2 � v2 ����� 2 �∂ � q1 � q2 � (8)

and ��� S1 � S2 ��� 2 is the sumof the squaresof the differencesofthe of the surfaceevaluations. Recallingthat ∂S

∂q is obtainable(Section5),

∂ %�% S1 ( S2 '+* % S1 ( S2 '�'∂q �

∂ % S1S1 ( 2S1S2 & S2S2 '∂q �

2∂ % S1 '∂q S1 � 2 � ∂ % S1 '

∂q S2 � ∂ % S2 '∂q S1 � � 2∂ % S2 '

∂q S2,

The algebrafor the Jacobianof the tangency constraintismadestraightforward by the triple scalarproductrelationwiththedeterminantof amatrix.

a � b � c �-,,,,,,a1 a2 a3

b1 b2 b3

c1 c2 c3

,,,,,, (9)

wherewe seta � S2u � u2 � v2 � , b � S1

u � u1 � v1 � andc � S1v � u1 � v1 �

from Eqn.4. Now Ctan becomes,for d � S2v � u2 � v2 � ,

Ctan � �a1 � b2c3 � b3c2 � � a2 � b1c3 � b3c1 � � a3 � b1c2 � b2c1 �d1 � b2c3 � b3c2 � � d2 � b1c3 � b3c1 � � d3 � b1c2 � b2c1 � �

(10)and

∂Ctan

∂q1 . 2 /0111111112 a1q 3 b2c3 4 b3c2 5�6 a1 3 b2qc3 6 c3qb2 4 b3qc2 4 c2qb3 57�7�7 4 a2q 3 b1c3 4 b3c1 584 a2 3 b1qc3 6 c3qb1 4 b3qc1 4 c1qb3 57�7�7 6 a3q 3 b1c2 4 b2c1 596 a3 3 b1qc2 6 c2qb1 4 b2qc1 4 c1qb2 5

d1q 3 b2c3 4 b3c2 5�6 d1 3 b2qc3 6 c3qb2 4 b3qc2 4 c2qb3 57�7�7 4 d2q 3 b1c3 4 b3c1 584 d2 3 b1qc3 6 c3qb1 4 b3qc1 4 c1qb3 57�7�7 6 d3q 3 b1c2 4 b2c1 596 d3 3 b1qc2 6 c2qb1 4 b2qc1 4 c1qb2 5: ;;;;;;;;<

(11)

4

Otherjointshaveanadditionalrolling constraintcondition,

∂Croll

∂ � q1 � q2 � � FqG (12)

where

F � �S1u ��� S1qq � R1S1L � q1 � S2qq � R2S2L � q2 �S1v ��� S1qq � R1S1L � q1 � S2qq � R2S2L � q2 � � �

G � �q1

q2 � � (13)

andFq is ∂F∂q . Now noteCroll is a functionof timeaswell; there-

fore thetermsGt , Fqt , andCroll arerequiredin theequationsofmotionandfollow from Eqns.12and13.

7 Assembly Optimization for HapticsMethodshavebeendevelopedfor solvingor optimizinggeo-

metricconstraintsfor thepurposes,amongothers,of mechanismassemblyand toleranceanalysis. Artificial intelligencetech-niques[Kramer, 1992], constraintpropagation,degreeof free-domanalysis,nonlinearprogramming,algebraicgraphreduction[Bourma,1995],anddynamicforces[Barr, 1988]have all beenusedassolutionmethods.

A user’s handgraspinganobjectmayberepresentedasthegroundconstraintin an assembly. Thesolutionof the resultingsystemsolvesthegeneralizedinversekinematicsproblemwhichallows theuserto move piecesof theassemblyin aninteractiveway. A review of theformulationof simpleconstraintsin Carte-siancoordinatesis containedin [Shabana,1994].

Automatic assemblyoptimization for finding mechanicalconfigurationssatisfyingjoint constraintsC � q ��� 0, i.e. by New-ton’soptimizationmethod,

Cq∆q � � C � q � (14)

is usefulin assemblyanalysisto eliminatetheneedto pieceto-getherassembliesby hand. Solving for q givesa valid mecha-nismconfigurationwhich is requiredin Section8.

An iterative techniqueto approximatinga solutionis givenby

∆q � kCTq � � C � q ��� (15)

Intuitively, we maythink of Eqn.15 asa kind of spring. WhenoneusesLagrangemultipliersto enforceconstraints,multipliers

λ give riseto a force in configurationspaceof CTq λ. Thedirec-

tionsof theconstraintforceareCq. If we think of springstryingto enforcetheconstraints,andmake thespringforcebepropor-tionalto error(erroris theconstraintmanifoldC), wewouldhavein general

springf orce � � kCTq C

Whenforming C andCq, the changein rotationof a vec-tor with respectto the changein the coordinatesq is used.[Shabana,1994] derives such a term ∂R % q4 = = 6 ' v

∂q4 = = 6 . Optimization

methodsfor geometricsatisfactionproblemsuseof ∂R % q ' v∂q but

can have difficulties in most configurationswith local minimawhenEuleranglesareused.To circumvent local minimaprob-lems,we usea quaternion4-tupleto representorientation.Thestepsfor evaluatingthe rotation operator∂ % qvq>?'

∂q in Cq are de-tailedin theappendixonquaterniondifferentiation.

Becausequaternionsrotatearoundaconstantvector, gettingfrom oneorientationto anotherin the optimizationof the con-straintequationsof a mechanismby gradientdescentis essen-tially linearly interpolatingbetweenpositionandquaternionori-entations.WhenEuleranglesareused,thegradientusedin de-scendingtowardsa moreoptimalorientationmaybecomestuckin modes.Thosemodescancausefrequentlocalminimabut theyareavoidedwith theuseof quaternions.

The useof geometricconstraintswith quaternioncoordi-natesthereforereducesto applying forces somewhat like themethodsin [Barr, 1988]. We have found experimentallythat achoiceof k=0.1 is a high but stablevaluefor the examplesen-counteredsofar. Thisconstantisalimit onhow fasttheassemblyprocessconvergesto satisfaction.Becausethereis no directde-pendencebetweenthesizeof themechanismandk, theconstanttimealgorithmin thenext sectioncanbeapplied.

7.1 Massively Parallel OptimizationEqn. 15 is a sparsematrix multiplied by a columnvector.

The locationsof thedenseareasin thesparsematrix areknownandgeneratedfrom theconstraintrelationbetweenbodies.Eachelementin ∆q maybeevaluatedonaseparateprocessorby send-ing a row of CT

q anda copy of C to a differentprocessor. Sinceeachprocessorhasat most two or threenon-zeroareasin CT

q ,only threeareasof multiplicationneedto occurbetweenCT

q andC in practice.Theinterdependenceof datais notaproblem;datacanbe copiedor farmedout to eachprocessorandindependentresultscanbecollectedonahostprocessor. Thealgorithmthere-fore hasthe propertyin that it will run in constanttime givenenoughprocessors.

5

8 Motion EquationWecoupletheequationsof motionandtwo timederivatives

of theconstraintequationC � q � t �@� 0 asin

Cqq � � Ct

Cqq � � Ctt � � Cqq � qq � 2CqtqMq � CT

q λ � Qe � Qv

whereQe areexternalappliedforces,andQv arevelocitydependentterms[Shabana,1994]. CT

q λ is the joint constraintforce as shown in [Haug,1992]. The problemcan be solvedfor q and λ in linear time [Baraff, 1996]. This differential-algebraicsystemmaybeintegratedwith theuseof stabilizationtechniques[Baumgarte,1972], [Negrut,1997], [Ascher, 1995],[Potra,1995], [Campbell,1995].

It is desirableto have the massmatrix M and vector Qv

in termsof quaternionacceleration[Omelyan,1998]. Stepsto-ward the reformulationof the massmatrix andgeneralizedin-ertial forcesthat dependon velocity in termsof quaternionsbyusingtheangularvelocity relationwith quaternionsmaybede-rivedfollowing themannerin [Haug,1992].

9 ResultsTheconstraintoptimizationtechniqueshavebeendeveloped

within aprototypicalMatlabenvironmentandarebeingincorpo-ratedinto Utah’sAlpha 1 modelingenvironment[Cohen,1980].The constraintsfor lower-pair joints have beenimplementedina high performanceC++ compiledform. Whenthe operatorisa part of the assemblyoptimizationthrougha groundedfinger,usuallysingleiterationhasbeensufficient for theinteractive re-assembly. An updateof 30kHz is achievedfor small examples,wuchasthoseshown in Figs.6, 7.

The surfaceconstraintequationshave alsobeendevelopedunderMatlab,andarein theprocessof beingincorporatedinto acompiledform. Convergenceof a curve-curve contactexampleis shown in Fig. 8. Simplerconstraintequations,includingrev-olute,spherical,andprismaticjoints have beenimplementedtotestthedynamicssupportframework.

Figure 6. Steps of auto-assembly during optimization for a mechanism

with bendy, 90 degree pieces and 2 ground constraints.

The useof gradientdescentin the assemblyoptimizationrequiresmore than 15 iterationswhen links are very far away

as in the beginning of a demonstration. However, only 1 it-erationper servo cycle is requiredin the usualcasedueto thefact that so many updatesper secondareobtainedwith the lin-eartimealgorithmandthattheuser’shandcannotmovevery farin 10( 5 seconds.This is a performanceadvantagefor hapticsmethodsover quadraticallyconverging, cubic cost algorithms[SDFast,1990],[Garcia,1994],[Shabana,1994], [Nahvi, 1998],[Lenarcic,1998].

The avoidanceof local minima for largely unassembledparts is also an advantageas discussedin Section7. “Nice-looking” solutionsare obtainedin an attemptto meet the as-semblyconstraintsbecausetheoptimization“forces” propagatethroughoutthemechanism.

Figure 7. Assembly solution for a Stewart platform.

The assemblyoptimizationprocedureappliesto other de-sign variablesbesideslink positions. Flexible body coordi-nates,link length,andotherparametershavealsobeenoptimizedin this framework in a mannersimilar to [Ashrafiuon,1990],[Hansen,1995],[Zou, 1997].

Figure 8. Assembly sequence for curve-curve mechanism

[Erdman, 1993]. Both links are also constrained by two revolute

joints attached to ground, making it a 1 DOF planar mechanism.

10 ConclusionTheapproachadvocatedin thispaperfor constraintsatisfac-

tion, inversekinematics,and similar assemblyproblemsis theuseof constraintequationsandtheJacobianof constraintequa-tions. The advantageof this approachis a compactimplemen-tation, re-useof constraintsfor dynamics,computationexpense

6

linear in thenumberof constraints,andmassive parallelismforconstantasymptoticrunningtime.

Theformulationof surfacerolling constraintsin theframe-work of constraintJacobiansfor usein assemblyoptimizationandLagrangemultiplier dynamicshasbeenderived. Initial re-sultsdemonstratedbothfor this formulationhavebeenshown tohaveatractableparameterizationandsmallimplementation.Ad-ditional researchis neededto testthescalablecharacteristicsoftheapproachon largermechanisms.

11 Appendix: Quaternion Differential AlgebraThe computationalcost of memorycachingand multipli-

cationsin the operationsperformedby rotationmatrices,Eulerangles,Rodriguezparameters,and quaternionsare all similar[Funda,1990]. But the uniquerepresentationof orientationisbestaccomplishedwith quaternions.

Thequaternionrotationoperatesonavectorby:

q v qA = � q20 � q � q � v � 2q0q � v � 2q � q � v�

whereqA � q0 � q denotestheinverseof thequaternion.The rotation constraint of a unit quaternion is that��� qo � q2

x � q2y � q2

z ���B� 1. We could eliminateq0 asdependent

with q0 �C� 1 � q2x � q2

y � q2z � 1

2 at thispoint,but for bothdynamicsandassemblyconsiderations,the unit lengthconstraintis moreeasily introduced in the geometric constraint equation andJacobian.This allows us to ignorethe specialcasethat occurswhen the scalarquaternionpart is 0 and to obtain a simpledifferentiationoperationwith all four elementsthat avoids thesquareroot introducedabove. This importantoperationusedinsimple joints and morecomplicatedsurfaceconstraintsis nowgivenby

∂ % qvq>D'∂q �FEG qyvz � qzvy qxvx � qyvy � qzvz � �

q0vy � qzvx � qxvz � qxvy � q0vz � qyvx � � q0vz � qxvy � qyvx q0vy � qzvx � qxvz � �

q0vz � qxvy � qyvx � qzvx � q0vy � qxvz

qxvx � qyvy � qzvz � qzvy � q0vx � qyvz� qyvz � q0vx � qzvy qxvx � qyvy � qzvz

HI(16)

AcknowledgmentsTheauthorsthankDavid Johnson,ThomasThompson,and

Ali Nahvi for editing help. Thanksalsogo to the studentsandstaff of the Alpha 1 project, within which this work was de-veloped.Supportfor this researchwasprovidedby NSF GrantMIP-9420352,by DARPA grantF33615-96-C-5621,andby theNSFandDARPA ScienceandTechnologyCenterfor ComputerGraphicsandScientificVisualization(ASC-89-20219).

REFERENCES[Adams] AdamsReferenceManual,Adams/Solver, MechanicalDynamics,Inc.,Ann Arbor MI, 1991.[Cremer, 1996] Anitescu,M., Cremer, J.,andPotra,F., “Formu-lating 3D ContactDynamicsProblems,” Mech. Struct.& Mach.,vol. 24,no.4, pp.405-437,Nov. 1996.[Ascher, 1995] Ascher, U., Chin, H., Petzold, L., Reich, S.,“Stabilization of ConstrainedMechanicalSystemswith DAEsandInvariantManifolds,” Mech. Struct.& Mach., vol. 23,no.2,pp.135-157,1995.[Ashrafiuon,1990] Ashrafiuon,H., Mani,N., “AnalysisandOp-timal Designof SpatialMechanicalSystems,” in Jour. of Mech.Des.,vol. 112,no.2., June1990.[Baraff, 1994] Baraff, D., “FastContactForceComputationforNonpenetraingRigid Bodies,” in ComputerGraphicsProceed-ings,SIGGRAPH,Orlando,July24-29,1994.[Baraff, 1996] Baraff, D., “Linear Time DynamicsUsing La-grangeMultipliers,” in ComputerGraphicsProceedings,SIG-GRAPH,New Orleans,August4-9,1996.[Barr, 1992] Barr, A., Currin, G., Gabriel, S., Hughes, J.,“Smooth Interpolationof Orientationswith Angular VelocityConstraintsusingQuaternions,” in ComputerGraphicsProceed-ings,SIGGRAPH,Chicago,July26-31,pp.313-320,1992.[Barr, 1988] Barzel,R., Barr, A., “A Modeling SystemBasedOn DynamicConstraints,” in ComputerGraphicsProceedings,SIGGRAPH,pp.179-187,1988.[Baumgarte,1972] Baumgarte,J., “Stabilizationof constraintsandintegralsof motion in dynamicalsystems,” in Comp.Meth.in Appl.Mech. andEng.,, vol 1, pp 1-16,1972.[Bourma,1995] Bouma,W., Fudos,I., Hoffmann,C.M., “A Ge-ometricConstraintSolver,” in CAD,27,1995,pp.487-501.[Campbell,1995] Campbell,S.,“High-Index Dif ferentialAlge-braicEquations,” Mech. Struct.& Mach., vol. 23,no.2, pp.199-222,1995.[Cai, 1987] Cai, C., Roth, B., “On the spatialmotion of rigidbodieswith point contact,” in Int. Conf. Roboticsand Automa-tion, pp.686-695,March1987.[Cheney, 1985] Cheney, W., Kincaid,D., NumericalMathemat-ics andComputing, Brooks/ColePublishing,1985.[Cohen,1980] Cohen,E., Lyche,T., andRiesenfeld,R., “Dis-creteB-SplinesAnd SubdivisionTechniquesIn ComputerAidedGeometricDesignAnd ComputerGraphics,” ComputerGraph-ics andImageProcessing, Vol 14,Number2, October1980.[Denavit, 1955] Denavit, J., Hartenberg, R., “A KinematicNo-tation for Lower Pair Mechanismsbasedon Matrices,” ASMEJour. of Appl.Mech.,vol. 22E,1955.[McCarthy, 1991] Dooley, J.R., McCarthy, J.M., “Spatialrigidbodydynamicsusingdualquaternioncomponents,” in IEEEInt.Conf. onRoboticsandAutomation, April, 1991,vol. 1, 1991.[Erdman,1993] Erdman,A., Editor, ModernKinematics: De-velopmentsin theLastForty Years,JohnWiley & Sons,1993.[Funda,1990] Funda, J., Taylor, R.H., and Paul, R.P., “On

7

homogeneoustransforms,quaternions,and computationaleffi-ciency,” in IEEE Trans.Autom.and Rob., vol 6, pp. 382-388,1990.[Garcia,1994] Garciade Jalon,J., et. al., “Kinematic andDy-namic Simulation of Rigid and Flexible Systemswith FullyCartesianCoordinates,” in Computer-Aided Analysis of Rigidand Flexible MechanicalSystems,Pereira,Ambrosio, editors,Kluwer AcademicPublishers,pp.290-292,1994.[Goldstein,1980] Goldstein,H., ClassicalMechanics,2nd ed.,Addison-Wesley, 1980.[Han,1998] Han,L., Trinkle,J.,“The InstantaneousKinematicsof Manipulation,” in Int. Conf. on RoboticsandAutomation,pp.1944-1949.,1998.[Hansen,1995] Hansen,J., “Synthesisof SpatialMechanismsUsing Optimization and ContinuationMethods,” Periera,M.,Ambrosio,J., eds,ComputationalDynamicsin Multibody Sys-tems,Kluwer AcademicPublishers,1995.[Haug,1992] Haug,E., Interm.Dynamics,PrenticeHall, 1992.[Johnson,1998] Johnson,D., Dissertation,Univ. of Utah,1998.[Jia,1998] Jia, Y., Erdmann,M., “ObservingPoseandMotionthroughContact,” in Int. Conf. Roboticsand Automation,pp.723-729.,1998.[Jones,1997] Jones,R., “Constraint-BasedInteractive Assem-bly Planning,” Int. Conf. on Roboticsand Automation,pp. 913-918.1997.[Kramer, 1992] Kramer, G.A., “A GeometricConstraintEn-gine,” in Artificial Intelligence, 58,pp.327-360,1992.[Kriezis, 1992] Kriezis, G., Patrikalakis,N., Wolder, F., “Topo-logical and differential-equationmethodsfor surface intersec-tions,” in CAD,vol. 24,no.1, pp.41-51,Jan.1992.[Lenarcic,1998] Lenarcic, J., “Alternative ComputationalSchemeof Manipulator InverseKinematics”, in Int. Conf. onRoboticsandAutomation,pp.3235-3240.1998.[Canny, 1990] Li, Z., Canny, J., “Motion of Two Rigid Bodieswith Rolling Contact,” in IEEE Trans.on Rob. andAutom., vol.6, no.1, 1990.[Lotstedt,1982] Lotstedt,P., “Mechanicalsystemsof rigid bod-ies subject to unilateral constraints,” in SIAM Jour. of Appl.Math.,vol. 42,pp.281-296,April 1992.[Martin, 1969] Martin, G., Kinematicsand Dynamicsof Ma-chines,McGraw-Hill, 1969.[Maekawa,1997] Maekawa, H., Tanie, K., Komoriya, K.,“Kinematics,Statics,andStiffnessEffectof 3D Graspby Multi-fingeredHand with Rolling Contactat the Fingertip,” in Int.Conf. onRoboticsandAutomation,pp.78-83.[Metaxas,1992] Metaxas,D., Terzopoulos,D., “Dynamic De-formation of Solid Primitives with Constraints,” in ComputerGraphicsProceedings,SIGGRAPH,Chicago,July26-31,1992.[Montana,1988] Montana,D., “The Kinematicsof ContactandGrasp”,in Int. Jour. of Rob. Res.,vol. 7, no.3, June1988.[Nahvi, 1998] Nahvi, A., Nelson,D., Hollerbach,H., Johnson,D., “Haptic Manipulationof VirtualMechanismsfrom Mechani-

calCAD Designs,” Int. Conf. onRoboticsandAutomation,1998.[Negrut,1997] Haug,E.,Negrut,D., Iancu,M., “A State-Space-BasedImplicit IntegrationAlgorithm for Dif ferential-AlgebraicEquationsof Multibody Dynamics,” Mech. Struct.& Mach., vol.25.,pp.311-334,1997.[Omelyan,1998] Omelyan, I., “On the numerical integrationof motion for rigid polyatomics: The modified quaternionap-proach,” Computers in Physics,Feb. 1998.[Papalambros,1995] Papalambros,P., “Optimal Designof Me-chanicalEngineeringSystems,” in Jour. of Mech. Des.vol. 117,no.2, June1995.[Petzold,1994] Petzold,L., “ComputationalChallengesin Me-chanicalSystemsFormulation,” in Computer-Aided AnalysisofRigid andFlexible MechanicalSystems,Pereira,Ambrosio,edi-tors,Kluwer AcademicPublishers,1994.[ProE] Pro/EngineerUser’sManual,version18.[Potra,1995] Potra,F., “Runge-KuttaIntegratorsfor MultibodyDynamics,” Mech. Struct.& Mach., vol. 23,no.2, pp.181-197,1995.[Sarkar, 1994] Sarkar, N., Yun,X., Kumar, V., “Control of Me-chanicalSystemswith Rolling Constraints:Application to Dy-namicControlof Mobile Robots,” in Int. Jour. of Rob. Res., vol13,no.1, Feb. 1994,pp.55-69.[Sciavicco,1996] Sciavocco, L., Siciliano, B., Modeling andControlof RobotManipulators,McGraw-Hill, 1996.[SDFast,1990] SDFast user’s manual, Symbolic Dynamics,1990.[Shabana,1994] Shabana,A., ComputationalDynamics,JohnWiley & Sons,New York, NY, 1994.[Shiller, 1993] Shiller, Z., Sundar, S.,“Designof Multi-Degree-of-FreedomMechanismsfor OptimalDynamicPerformance,” inJour. of Mech. Des.,vol. 115,no.2, 1993.[Shoemake,1985] Shoemake, D., “Animating rotation withquaternioncurves,” in ComputerGraphics Proceedings,SIG-GRAPH,pp.245-254,July, 1985.[Snyder, 1993] Snyder, J.,Woodbury, A., Fleischer, K., Currin,B., Barr, A., “Interval Methodsfor Multi-Point Collisionsbe-tweenTime-DependentCurved Surfaces,” ComputerGraphicsProceedings,SIGGRAPH,1993.[Yang,1995] Yang, K., Lee, S., Bae, D., Suh, M., “Use ofJointGeometricConditionsin FormulatingCartesianConstraintEquations,” Mech. Struct.& Mach., vol. 23, no.3, pp.395-417,1995.[Yun,1995] Yun,X., “StateSpaceRepresentationof Holonomicand NonholonomicConstraintsResulting from Rolling Con-tacts,” in IEEEInt. Conf. onRoboticsandAutomation, 1995,pp.2690-2694.[Zou, 1997] Zou,H., Abdel-Malek.K., Wang,J.,“A VariationalApproachfor theDesignof SpatialFour-BarMechanism,” Mech.Struct.& Mach., vol. 25.,pp.41-59,1997.

8