Srinivasa Dave Ferguson James Kuffner The Robotics Institute Intel Research Pittsburgh Carnegie Mellon University 4720 Forbes Ave Pittsburgh PA USA Pittsburgh PA 15213 USA ridankov kuffner cscmuedu siddharthasrinivasa daveferguson intelcom Abstract ID: 64664
Download Pdf The PPT/PDF document "Manipulation Planning with Caging Grasps..." is the property of its rightful owner. Permission is granted to download and print the materials on this web site for personal, non-commercial use only, and to display it on your personal computer provided you do not modify the materials and that you retain all copyright notices contained in the materials. By downloading content from our website, you accept the terms of this agreement.
betakentoensurethattheobjectdoesnotslipoutoftherobotend-effector.Ofgreaterconcernisthefactthattherenolongerexistsaone-to-onemappingfromrobotmotiontoobjectmotion:sincetheobjectislooselycaged,therecanexistend-effectormotionsthatproducenoobjectmotion,andobjectmotionwithoutexplicitend-effectormotion.Theplannerproposedinthispaperusesaremarkablysimpleyeteffectivetechniquetonarrowthegraspsetchoicesasitmovestowardsthegoalstate.Theplannerworksproducesarmmotionsthathaveahighprobabilityofaccomplishingthetaskregardlessoftheuncertaintyintheobjectmotion.II.RELATEDWORKOurworkbuildsuponrelatedworkintwoareasofmanip-ulation:caging,andtaskconstrainedmanipulation.Earlyworkoncaging[6]consideredtheproblemofdesign-ingalgorithmsforcapturingapolytopeusingagivennumberofpoints.Sincethen,therehavebeenseveralapplicationstocooperativemanipulationaswellasgrasping.Pereira,KumarandCampos[7]proposeddecentralizedalgorithmsforplanarmanipulationviacagingusingmultiplerobotspushingtheobject.RimonandBlake[2],[3]viewedcagingasanintermediatesteptoimmobilizinganobjectandcomputedcagingsetsthatwouldleadtoapre-speciedimmobilizationgrasp.Sudsang,Ponce,andSrinivasa[8]introducedamorerelaxednotionofcaptureregions,placingngerswheretheobjectcouldbepreventedfromescapingtoinnity.Astate-of-the-artcagesynthesisalgorithmandsurveyofrecentresultsincagingmaybefoundinVahediandVanderStappen[9].Oneoftherstformulationsoftask-constrainedmanipula-tionwasprovidedbyMason[1]whoobservedthatmotionalongtaskconstraintswhichproducedconguration-spacesurfacesorC-surfacesrequiredthecombinationofpositioncontroltomovealongtheC-surface,andforcecontroltoguaranteecontactwiththesurface,whichhetermedcompliantmotion.Heproposedaformalismthatcombinedthenaturalconstraintspresentedbythetaskandthedesiredgoaltrajectorytoproducecontrolpoliciesintermsofarticialconstraints.Therehasbeenavastamountofliteraturefollowingthiswork[10],[11],[12],[13].Usuallysolvingataskconstrainedproblemistightlycou-pledwithsimultaneouslysolvingthecompliantcontrolandvisualservoingproblems.[14]implementabehavioralmodulethatscriptsthegeneraltaskofopeningadoorwhilebeingcomplianttounknownvariablesatrun-timelikedirectiontoopenthedoorandturnthehandle.[15],[16]proposeaframeworktosimultaneouslysolvethetaskbycontrollingforcesandvelocitiesthroughavisualservoloop.PerhapstheworkthatisclosestinspirittooursisthatofPratset.al.[17]whoallowtheend-effectortomovefreelyalongcertaindirectionsduringmanipulations.Onelimitingfactoristhatthesedirectionsarecarefullychosenbyhandtoensurethattheydonotaffecttheoveralltask.Infact,end-effectorsdonotalwayshavetocagetheobject;aslongasthetargetobjectmovesinthedesireddirection,justconsid-eringpushingcanalsoincreasethefreespaceoftherobot [18].Webelieveourworkisageneralizationoftheaboveideas:insteadofspecicallyparameterizingtherelationshipbetweentheend-effectorandtheobjectusingsimplerules,weautomaticallygenerateadata-drivenrepresentationofthisrelationship:thecagingmanifold.Themaincontributionofourworkisarelaxationofthetaskconstraintframeworkusingcagingandtwoalgorithmsforplanningunderthisframework.Inthispaper,wedeneacageastheconditionwherearobothandconstrainsthecongurationspaceofanobjecttoanitevolume.Inourcase,weareinterestedinkeepingthesizeofthisvolumesmallsotheobjectcanbecontrolledwiththehand.Thecongurationspaceoftheobjectitselfcanbeonedegreeoffreedomforadoorhinge,aposeinthe2Dplane,oraposein3D.III.RELAXEDPROBLEMFORMULATIONWeformulatetheproblemusingthecongurationspaceoftherobotq2Q,thecongurationspaceoftheend-effectorg2G,andthecongurationoftheconstrainedtargetobject2R.Eachofthesespacesisendowedwithitscorrespondingdistancemetricd:XX!R.Werepresentg,henceforthtermedagrasp,asthe6Dposeoftheend-effectorinSE(3).Althoughourproposedmethodisgeneralenoughtoincludejointsinthegraspconguration,weassumethatthehandjointsdonotmovewhileplanning.Theassumptionismandatedbyourphysicalsetupwhichdoesnotprovideaccuratesynchronizationofarmandend-effectormotion.Intherelaxedtaskconstraintformulation,eachtargetobjectisendowedwithataskframewhichisrigidlyattachedtoit,andasetofgraspsGrepresentedinthattaskframe.ThesetGiscarefullychosentoensurethatanygraspisguaranteedtocagetheobject.IfwedeneRgtobethesetoftargetcongurationsreachableunderagraspg,thenthetargetatcongurationiscagedbytherobotif2RgRandeverypointonthetheboundaryofRgisincollisionwiththeend-effectoratposeg.Althoughthisisaconservativedenitionofacage,itisnecessarybecauseend-effectoristheonlyphysicalbodyknownwithcertaintyandcagingshouldbeenvironmentindependent.NotethatalimitingcaseofacageisaformclosuregraspwhereRg=fg.Incongruencewiththetraditionaltaskconstraintformula-tion,wedescribetheposeofgraspsinGwithrespecttoacoordinateframethatisrigidlyattachedtotheobject,termedthetaskframe.AtransformTrelatesthetaskframeatanobjectcongurationtotheworldreferenceframe.Theutilityofthisrepresentationarisesfromtheobservationthat,underarigidgrasp,theposeoftheend-effectorisinvariantinthetaskframe.ThisallowsustocomputeandcacheGofine,therebyimprovingtheefciencyoftheonlinesearch.Atanyconguration,wedenotethegraspsetintheworldframebyTG=fTgjg2Gg:(1)Givenagraspgwedenethesetof(inversekinematics) Fig.3.GraspSetgeneratedfortheManusHand. thecollisionfreecaginggraspscanbedependentonthecongurationofthetarget,thegraspgenerationprocessrunsmultipleRRTsatmultipletargetcongurations.Theunionofallthecomputedgraspsistakenandasubsetspanningthegraspspaceisextracted.B.GeneratingaContactGraspSetInordertoguaranteethatanend-effectorposeisabletomovethetargetobjecttoitsnaldestination,theend-effectorhastoexertforcesthroughcontacttokeepthetargetobjectclosetothatconguration.WecallthissetGcontact.Avalidcontactgraspisonewhereweareunabletomovetheobjectasmallstepwithoutcollidingwiththegrasp.Thus,thecontactgraspapproachesform-closure.ThismaybeformalizedasGcontact=fg2Gj82R;d(RTg)g(8)whered(R)=max12R;22Rd(1;2)(9)isthemaximumdistancebetweenanytwocongurationsinR.Fig.1showstheresultsoftheRRTexploration,pruning,andnallythegraspspickedforthecontactset.Fig.3showthegraspsetcomputedfortheManusHand.V.PLANNINGWITHRELAXEDCONSTRAINTSWedescribetwoplanningalgorithmstosolvetherelaxedconstraintproblem:adiscretizedversionandarandomizedversion.Therandomizedalgorithmismoreexibleandmakeslessassumptionsabouttheproblemstatement,howeverthediscretizedalgorithmissimpletoimplementandusefulforexplainingtheconceptsbehindrelaxedplanning(aswellasthemotivationforarandomizedalgorithm). Fig.4.Thebasicframeworkusedforplanningdiscretepathsfqigjn1inrobotcongurationspacetosatisfypathsfigjn1inobjectcongurationspace. A.DiscreteFormulationTheunderlyingassumptionofthediscreteformulationisthatadesiredpathofthetargetobjectisspecied.Specifyingthepathintheobject'scongurationspaceasaninputtotheplanneristrivialforhighlyconstrainedobjectslikedoors,handles,cabinets,andlevers.Thecongurationspaceoftheseobjectsisonedimensional,sospecifyingapathfromatobiseasilydonebydisretizingthatpathintonpoints.Inthemoregeneralcasewhereanobject'scongurationspacecanbemorecomplex,wedenoteitsdesiredpathasfigjn1whereeachofthecongurationsihavetobevisitedbytheobjectinthatorder.Thediscreterelaxedconstrainedproblemisthenstatedas:givenadiscretizedobjectcongurationspacepathfigjn1,ndacorrespondingrobotcongurationspacepathfqigjn1suchthat81in(i;qi)2Cfree(10)FK(qn)2TnGcontact(11)81nd(FK(qi�1);FK(qi))1(12)81nd(qi�1;qi)2(13)whereEqn.10andEqn.11constraintheend-effectortolieinthecurrentgraspsetdenedfortheobjectandEqn.11guar-anteesthenalgraspisincontact.Tosatisfythecontinuityconstraintontherobotcongurationspacepath,Eqn.12andEqn.13ensurethatadjacentrobotandgraspcongurationsareclosetoeachother.AstraightforwarddiscreteplanningapproachtosolvethisproblemisprovidedinAlgorithm1.Webeginbyrstrunningafeasibilitytestthroughtheentireobjecttrajectory.Thisstepisalsousedtoinitializethegraspandkinematicsstructuresusedforcaching.Weassumeaninversekinematicssolverispresentforeveryarm.Furthermore,ifthearmisredundantthesolverwillreturnallsolutionswithinadiscretizationlevel.Wecomputethesetofcontactgraspsthatwillkeeptheobjectinform-closureatitsdesirednaldestinationn(line11).ForeachgraspinthissetwecomputeIKsolutionsforthecompletecongurationoftherobot,andforeachIKsolutionweattempttoplanapaththroughcongurationspacethat Fig.7.WAMarmusedtoopenakitchencupboard. orless,enumeratingallIKsolutionsisn'taproblem.However,assoonasthejointsincreaseoramobilebaseisconsidered,thediscretizationrequiredforIK(g)toreasonablyllthenullspacegrowsexponentially.Second,thedesiredobjecttrajectoryisxed,whicheliminatesthepossibilityofmovingthedoorinonedirectionandthenanothertoaccomplishthetask(see[12]foranexamplewherethisisrequired).Toovercometheselimitations,wealsoappliedarandom-izedplannertotheproblem.WechosetheRandomizedA*algorithm[20],whichoperatesinasimilarfashiontoA*exceptthatitgeneratesarandomsetofactionsfromeachstatevisitedinsteadofusingaxedset.RandomizedA*iswellsuitedtoourcurrentproblembecauseitcanusethetargetobjectdistancetogoalasaheuristictofocusitssearch,itcanguaranteeeachstateisvisitedatmostonce,itdoesnotneedtogeneratealltheIKsolutionsforagivengrasp,anditcanreturnfailurewhennosolutionispossible.ThekeydifferencebetweenRandomizedA*andregularA*isthesamplingfunctionusedtogenerateneighborsduringthesearch.Forourrelaxedconstraintsproblemthetaskofthissamplingfunctionistoselectarandomconguration(new;qnew)andarandomgraspgnew2TnewGsuchthatqnew2IK(gnew).Ideally,thisshouldbedoneefcientlywithoutwastingtimeconsideringsamplespreviouslyrejectedforthesameconguration.TheA*criteriawillensurethatthesamecongurationisn'tre-visitedandthatthereisprogressmadetowardsthegoal,sothesamplingfunctionneedsonlyreturnarandomcongurationinCfreearoundthecurrentconguration(;q)asfastaspossible.Algorithm2providesourimplementationofthesamplefunction.Itrstsamplesatargetobjectcongurationnewclosetothecurrentconguration(line3),thensearchesforfeasiblegraspsfromthenewgraspsetTnewG0(line6),andthensamplesacollision-freeIKsolutionclosetoq(line8).Inordertoguaranteewesampletheentirespace,RANDOMCLOSECONFIGshoulddiscretizethesamplingspaceofthetargetcongurationsothatthenumberofdistinctnewthatareproducedissmall.Thisisnecessarytoensurethatsamplingwithoutreplacementisefcient.Eachtimeasampleischosen(line6),itisremovedfromGnewsoitisneverconsideredagain,anoperationthattakesconstanttime.IfthetargetisclosetoitsgoalthenG0isthecontactgraspsetGcontact,otherwiseG0istheregulargraspsetG.Oncea Algorithm2:fnew;qnewg SAMPLENN(,q) G ;,qnew ;1whileqnew=;do2new RANDOMCLOSECONFIG()3ifnotEXIST(Gnew)then4Gnew TnewG05gnew SAMPLEWITHOUTREPLACEMENT(Gnew)6ifgnew6=;then7qnew SAMPLEIK(gnew,q)8elseifCHECKTERMINATION()then9returnf;;;g10end11returnfnew;qnewg12 DiscreteRandomized 6DOFManusArm441%503%6DOFPumaArm130%126%7DOFBarrettWAM13%24%7DOFBarrettWAM(Harder)163%162% TABLEIIINCREASEINFEASIBILITYSPACEWHENUSINGRELAXEDPLANNINGCOMPAREDTOFIXED-GRASPPLANNING. graspisfound,SAMPLEIKsamplesthenullspaceoftheIKsolveraroundquntilacollision-freesolutionisfound.Ifnot,theentireprocessrepeatsagain.Ifallgraspsareexhaustedforaparticulartargetconguration,thesamplerchecksforterminationconditionsandreturnsfalse(line9).VI.EXPERIMENTSTheroboticsimulationenvironmentweusedtoperformallplanning,testing,andrealrobotcontrolisOpenRAVE[21],theOpen-SourceCross-PlatformRoboticsVirtualEnvironment.Totesttheperformanceofthealgorithm,theplanningtimesandfeasibilityregionsarecalculatedforthreedifferentrobotsinvariousscenes(Fig.5,Fig.7).Allthehandlesofthetargetobjectsaremeasuredcarefullyfromtheirreal-worldcounter-parts.Thetasksareasfollows: TheManusArmisrequiredtoopenthedoor90degrees. #TrialsGraspSetSizeDiscrete(Successes)Discrete(Failures)Randomized(Successes)Randomized(Failures) 6DOFManusArm77845500.235s0.234s0.143s0.23s6DOFPumaArm67553001.49s0.043s1.83s0.028s7DOFBarrettWAM273427610.58.4310.537.47DOFBarrettWAM(Harder)24221230.116s0.021s0.209s0.029s TABLEISTATISTICSFORTHESCENESTESTEDSHOWINGAVERAGEPLANNINGTIMES(INSECONDS)ANDSIZEOFTHEGRASPSETSUSED. Fig.8.WAMarmautonomouslyopeningacupboard,puttinginacup,andclosingit.Wallclocktimesfromstartofplanningareshownineachframe. ThePumaArmisrequiredtoopenthecupboard115degrees. TheWAMarmisrequiredtoopentheclosercabinet90degreesandthefarthercabinet60degrees.Ineachscene,therobotisrandomlypositionedandorientedontheoor,andthentheplannersareexecuted.Thousandsofrandompositionsaretestedineachscenetocalculateaveragerunningtimes(TableI).Theparametersfortherandomizedalgorithmstayedthesameacrossallrobots.NotethattheplanningtimesfortheeasierWAMscenearemuchhigherthantherestofthescenes.Thisshowsthatthemoreopenthespaceis,thelongerittakestosearchforallpossiblesolutions,andespeciallylongertodeclarefailurewhenasolutiondoesn'texist.Toshowthatrelaxedgraspsetsreallydoincreasetheregionsthearmcanachieveitstaskfrom,wecomparethefeasibilityregionsproducedwiththerelaxedgraspsetmethodandthexedgraspmethod.Thexedgraspmethodusesasingletask-framegraspthroughouttheentiresearchprocess.Tomakethingsfair,wetryeverygraspinGcontactbeforedeclaringthatthexedgraspmethodfails.TableIIshowshowmanytimesthefeasibilityregionincreasedfortherelaxedmethodscomparedtothexedmethod.Asexpected,thelowerdimensionalmanipulatorsbenetgreatlyfromrelaxedtaskconstraints.Furthermore,thedoorcanbeopenedmuchfurtherusingtherelaxedapproachthanwiththexedgraspmethod.Therandomizedalgorithm'simprovementoverthexedmethodisoccasionallylessthanthatofthediscrete algorithmbecauseweareusingearlyterminationcriteria;runningthealgorithmforlongerproducesfeasibilityregionsthataregreaterthanorequaltowhatthediscretealgorithmproduces.Fig.6showsthefeasibilityregionsineachscenebetweenrelaxedgraspsandxedgrasps.Realexperimentsweredoneontworobots:theManusArmonawheelchairopeningadoor(Fig.9),andtheBarrettWAMputtingcupsinacupboard(Fig.8).ItisimpossibletoopenthedoorsowidewiththeManusArmwithoutconsideringrelaxedgraspsbecausethereachabilityissolow.TheexperimentweperformedwiththeWAMistoautonomouslyopenacabinet,putacupinsideit,andcloseit.Therobotautonomouslyplannedforcollision-freeandreachablegraspswhenpickingupthecupusingthegraspplanningframeworkproposedby[22].Itshouldbenotedthatthenaldestinationisverytight,buttheplannerwasabletondasolutionandtherobotsuccessfullycompletedexecutionoftheentiretaskinacombinedtimeof1minuteand58seconds.VII.CONCLUSIONSANDFUTUREWORKWehaveproposedamotionplanningmethodforcon-strainedmanipulationtasksthatcombinesobject“caginggrasps”andefcientsearchalgorithmstoproducemotionplansthatsatisfythetaskconstraints.Theeffectivenessofourapproachhasbeenillustratedbyexperimentalresultsontwodifferentreal-worldautonomousmanipulationtasks.Relaxingthetaskconstraintscangivethearmmorechancestonishthetaskwithoutrelyingonsynchronizationwith