Hierarchical Task and Motion Planning in the Now Leslie Pack Kaelbling and Tom as LozanoP - PDF document

generateaplanconsistingofrobottrajectoriesandgraspingoperationsthatwillresultinthedesiredconguration[2],[3].Planninginhybridspaces,combiningdiscretemodeswitchingwithcontinuousgeometry,canbeusedtosequencerobotmotionsinvolvingdifferentcontactstatesordynamics.HauserandLatombe[4]havetakenthisapproachtocon-structclimbingrobots.Planningamongmovableobstaclesgeneralizesmanipu-lationplanningtosituationsinwhichadditionalobstaclesmustbemovedoutofthewayinorderforamanipulationormotiongoaltobeachieved.Inthisarea,theworkofStilmanetal.[5],[6]takesanapproachsimilartoours,inthatitplansbackwardsfromthenalgoalandusessweptvolumestodetermine,recursively,whichadditionalobjectsmustbemoved.Oursolutiontotheproblemofmovableobstaclesarisesfromageneralregression-basedsymbolicplanner,but,inthecurrentimplementation,isnotguaranteedtondasolutionwheneveroneexists.IntegratingsymbolicandmotionplanningTheneedforintegratinggeometricandtaskplanninghaslongbeenrecognized[7].IntheworkofCambonetal.[8],asymbolicdomainactsasaconstraintandprovidesaheuristicfunctionforacompletegeometricplanner.PlakuandHeger[9]extendthisapproachtohandlerobotswithdifferentialconstraintsandprovideautility-drivensearchstrategy.HierarchicalplanningHierarchicalapproachestoplan-ninghavebeenproposedsincetheearliestworkofSac-erdoti[10],whoseABSTRIPSmethodgeneratedahi-erarchybyleavingoffpreconditions.Marthietal.[11]givehierarchicaldomaindescriptionssemanticsbasedonangelicnon-determinism,andcandramaticallyspeedupthesearchforoptimalplansbasedonupperandlowerboundsonthevalueofrenementsofabstractoperators.Goldman[12]givesanalternativesemanticsthatincorpo-ratesangelicnon-determinismduringplanningandadver-sarialnon-determinismduringexecution.Nourbakhsh[13]suggestsahierarchicalapproachtointerleavingplanningandexecutionthatissimilartoours,butdoesnotintegrategeometricreasoning.TheworkofWolfeetal.[14]providesahierarchicalcombinedtaskandmotionplannerbasedonhierarchicaltransitionnetworks(HTNs)andappliesittoamanipulation-planningproblem.III.EXAMPLEConsiderthedomainshowningure1.1.ThegoalisfortheobjectlabeledAtobecleanandputawayinthestorageroom.TherobotmusttakeA,putitintothewasher,washit,andthenmoveittothestorageroom.Accomplishingthisrequiresmovingotherobjects.Inthissection,wedescribeinformallyhowthisproblemissolvedbyoursystem.Theinitialstateisgivenasathree-dimensionalgeometricmodel(thegureshereareshownlookingdownfromabove.)Thegoalisspeciedasaconjunction:In(a;storage)^Clean(a).Arecursiveprocessofplanningandexecutiontakesplace,asshowningure2.1.Bluenodesinthetree,labeledwithnumbers,denoteplanningproblems.Therstplanningproblemisthetop-levelgoal,whichisrstaddressedwithabstractversionsoftheoperators.Operatorsareabstractedbypostponingpreconditions;wemakethemprogressivelymoreconcretebyrequiringmorepreconditionstohold.Forthisgoal,atwo-stepplanismade;itisshownastwodescendantpurplenodes,eachofwhichrepresentsanoperation(thenotationAimeansthattheoperatorisatabstractionleveli.)Theplanistorunthewasherwithainit,andthentoplaceaintothestorageregion.Thatplanisrecursivelyexecuted,byplanningforandexecutingeachofitsoperationsinturn.Ifanoperationisaprimitiveaction,thenitisexecuteddirectly;otherwise,asubgoalisconstructed,consistingoftheconditionsnecessarytoguaranteethattherestofthehigh-levelplanwillsucceed,andaplanismadeforthatsubgoal.Here,theabstractWash(a)operationisrenedintothesubgoalClean(a).2.WenowplanforthegoalClean(a),generatingaplanwithtwooperations.TheWashoperatorisconsideredmoreconcretely,soweplantosatisfyitsprecondition,thatabeinthewasher,byanabstractplaceoperationthatputsaintothewasher.3.Weexpandtherstoperation,planningtopickaupfromitsstartinglocationandplaceitintothewasher.4.Now,weplantosatisfythegoalofholdingobjecta.Theresultingplanhastwosteps.Therstrequiresthatasweptvolumeoftherobotmovingtoobjectaandpickingitupbefree.Thesweptvolumeisshowningure3.1asacomplexbrownpolygon;itwascomputedusingafastplannerthatconsideredonlytranslationsoftheobject,withagripperattachedtoit,andoftherobotbase.Itreturnstheunionofthesweptvolumesofthebase,thegripper,andtheobject.Thesecondoperationisaconcretepickofobjecta.5.Thenextsubgoalnowincludesallofthepreconditionsforthepickoperationtosucceed:therobotneedstobeholdingnothing,thesweptvolumeforaneedstobeclear,andobjectashouldbeinitsstartingplace(thatistheplaceforwhichthesweptvolumewascomputed;ifaismoved,thenclearingthesweptvolumewejustcomputedwillnotnecessarilysufce.)Theresultingplaniscomprisedofabstractoperationstoremovebothbandcfromthesweptvolume.Becauseourcostmodelisstillsomewhatweak,itchoosestoremovebrst.6.Toremovebfromthesweptvolume,aparkingplace,shownasPBingure3.1,issuggested.Thesuggestionisguaranteednottoconictwithpickinga.Theplannernowdeterminesthatcisinthesweptvolumeofb,andndsaparkingplacePCforit,asshowningure3.2.Theplanistopickandplacecandthentopickandplaceb.Theoperationtopickcisrenedintoaprimitiveoperation.Atthispoint,agrasplocationisselectedandarobotmotionplanner(inthiscase,asimpleRRTimplementation)iscalledtoplanthepickoperation.Theprimitiveoperationisexecutedintheworld,whichresultsintherobotgraspingc.Thenitplanstoplacecintheparkingplaceandexecutesthatplan,withtheresultshowningure1.2.Similarly,adetailedmotionformovingbisplannedandexecutedintheworld,resultingingure1.3.Wecontinuewiththerecursiveexecutionofthe Fig.2.Planningandexecutiontreeforwashingandputtingawayanobject. Fig.3.Suggestionsforsweptpathsandparkinglocations.Theregionsweptbisclearinthestartingstate,soitneverappearsintheplanningsubgoals. characterizationoftheworldstateintermsofuents;thisisimportantbecausethesetofpossiblegeometricregionsthatcouldserveasargumentstoauentisinnite.Theonlyrequirementisthat,foreachuenttype,thereisaprocedurethatwilltakealistofgroundargumentsandreturnthevalueofthatuentintheworldstate.Inourimplementation,theworldstateisrepresentedbyacongurationoftherobotandasetofobjects,eachofwhichhasattributesincludingpose,shape(unionofconvexpolyhedrathatareextrusionsinz),whetheritisgraspedbytherobot,andwhetheritisclean.GoalsAgoalforourplanningandexecutionsystemisasetofworldstates,describedusingaconjunctionofgrounduents.Thegoalofhavingobjectatobecleanandinthewashercanbearticulatedas:In(a;washer)=True^Clean(a)=True:Duringthecourseofregression-basedplanningintermediategoalsarerepresentedasconjunctionsofuentsaswell.Wewillwrites2GtomeanthatworldstatesiscontainedinthesetofgoalstatesG.OperatorsAplanningdomainischaracterizedbyasetofprimitiveactions.Inourformulation,wehandlethelowestlevelofplanningwithaspecial-purposegeometricgraspandpathplannerfortherobot.Thus,forthepurposesoftherestoftheplanning,theprimitivesareactionsthatencapsulatetheplanningandexecutionoftwoprimaryoperations:Pick(O),whichcausestherobottomovetoobjectOandpickitup;andPlace(R),whichcausestherobottotakethecurrentlyheldobject,possiblymovetoanopenpartofthespaceandregraspit,andthenmovetoanappropriateposeandungrasptheobject,guaranteeingthattheobjectisentirelycontainedinregionR.Thereisoneadditionalprimitivethathasnogeometriccomponent:Wash()simplycausesthewashingmachinetoberun,andanyobjectsthatareinthewasherareawillbecomeclean.Eachoftheseoperationsischaracterizedbyoneormoreoperatordescriptions.Eachoperatordescriptioncanbeusedatmultiplelevelsofabstraction:webeginbydescribingthemintheirmostconcreteform.Inadiscretedomain,wecandenetheoperatorsinaSTRIPS-styleform:F(A1;:::;An)=V:exists:B1;:::;Bkpre:1;:::;msideEffects: 1;:::; lprim:cost:cwhereF(A1;:::;An)=Visthetargetuent,theAiandVarevariablesorconstants,theBiarevariables,theiand iareuentswhoseargumentsareconstantsorvariablesthatoccurasAsorBs,isaprimitiveaction,andcisapositiverealcost.Thisisanoperatorschemathatstandsforawholefamilyofgroundoperators,forallpossibleassignmentsofconstantvaluesinthedomaintovariablesinthetargetuentorintheexistslist.Thesemanticsoftheoperatordescriptionisthat,iftheprimitiveactionisexecutedinanyworldstatesinwhichalloftheuentshold,thentheresultingworldstatewillbethesameass,exceptthatanyuentmentionedasthetargetuentorasideeffectwillhavethevaluespeciedbythoseuents.Tooperateininnitedomains,weaugmentthestandardoperatordescriptionswiththefollowingfeatures:Suggesters,whichareproceduresthatmapcurrentstartandgoalstates,andbindingsofothervariables,torestricteddomainsforexistentialvariables.Thissignicantlydecreasesthebranchingfactorandincreasesthelikelihoodthatse-rializationwillsucceed,bymakingintelligentchoicesofbindingsforfreevariables.Agivensetofsuggestersisapplicabletoanydomaindescribedbythesamesetofgeometricoperatorsandgeneralrobottype,e.g.anarmonamobilebase.Proceduraloperatordenitions,whichmapvariablebind-ingsintolistsofpreconditions,sideeffects,newbindings,andcosts.Theseallowustocallthesuggestersexibly,dependingonwhichvariablesarebound,andtogeneratelistsofpreconditionsandside-effectswhoselengthvariesdependingonthesituation.Inferentialattachments,whicharetwotypesofproce-duresattachedtouents.Theentailsattachmentofuentcomputeswhetherlogicallyentailsanotheruent0.Ingoalregression,whenapplyinganoperationtoagoalg,thegoaluentandanysideeffectuentsarealwaysremovedfromg;inaddition,weremoveanyuentsingthatareentailedbythegoaluentoroneofthesideeffects.Thecontradictsattachmentofuentcomputeswhetherlogicallycontradictsanotheruent0.Ingoalregression,ifanyoftheconditionorside-effectuentscontractauentinthegoal,thentheoperationisnotconsidered.Non-deterministicsideeffects,whichmodelabstractactionswithnon-deterministiceffectsbysettingthevalueofside-effectuentstobeNone,indicatingthattheresultingvalueisunknown.Followingareoperatordescriptionsusedinourexampledomain.Thenumbersprecedingthepreconditionsrefertotheabstractionlevel;someirrelevantpreconditionshavebeenomittedforclarity.Thedescriptionsrefertogoal,whichisthecurrentregressionsubgoalintheplanningprocesstowhichthisoperatorisbeingappliedandtostartwhichisthecurrentworldstateatthetimethisparticularplanningproblemisbeingsolved,whichisnotnecessarilytheinitialstatefortheentireplanningandexecutionproblem.ThepickoperationresultsintherobotholdingobjectO:Holding()=O:dene:Ts=fT:ClearX(T;X)2goal^O62Xgexists:L2fLocation(O;start);SuggestParking(O;Ts;start)gP2SuggestPaths(O;L;home;start)pre:0.Holding()=nothing0.In(O;L)=True2.ClearX(sweptVol(P);[O])=TruesideEffects:8L0:In(O;L0)=Falseprim:Pick(O)Othervariablesthatdeterminethebehaviorofthisopera- addressedagainwithnoabstraction,andsolvedwithnofurtherattemptsatserialization.V.ALGORITHMSInterleavedplanningandexecutionArelativelystandardregression-planningalgorithm,basedonanA*searchthatworksbackwardfromthegoal,generatingsub-goalsthataretheweakestpreconditionofthegoalundereachapplicableaction,isusedtosolvesingleplanningsub-problems.Thearchitecturecanbethoughtofasdoingadepth-rsttraversalofaplanningtree,andisimplementedasarecursivealgorithm,asshownbelow.TheplanningandexecutionsystemisinvokedbycallingHPN(currentState;goal;operators;absLevel;world),wherecurrentStateisadescriptionofthecurrentstateofworld;goalisaconjunctionofuentsdescribingasetofgoalstates;operatorsisasetofhierarchicaloperatordescriptions;absLevelspecies,foranygrounduent,thenumberoftimesithasservedasaplanstepintheHPNcallstackaboveit;andworldisanactualrobotorasimulatorinwhichprimitiveactionscanbeexecuted.Inthispaperworldisageometricmotionplannercoupledwithanexecutioncapability.HPN(currentState;goal;operators;absLevel;world):ifholds(goal,currentState):returnTRUEelsep=PLAN(currentState,goal,operators,absLevel)for(oi;gi)inpifprim(oi):currentState=world:execute(oi)elseHPN(currentState,gi,operators,NEXTLEVEL(absLevel,oi),world)ThePLANprocedureiscalledwiththecurrentstate,thegoal,asetofoperators,andanabstractionlevel.Itreturnsalist((o1;g1);:::;(on;gn))wheretheoiareoperatorinstances,gn=goal,giistheweakestpreconditionofgi+1underoi+1,andstart2g0.Weusegoalregressionbecauseitcomputes,foreachplanstep,theweakestconjunctivesubgoalthatcanserveasthetargetfortheplanningproblemsatthenextleveldown.Inourimplementation,absLevelisadictionary,mappinggrounduentstonumericabstractionlevels.Wheneverwedescend,recursively,toaddressanewplanningproblem,theNEXTLEVELprocedureincrementstheabstractionlevelassociatedwiththatuent.SuggestersTheoperatordenitionsinourdomainusetwosuggesters:SuggestPathsandSuggestParking.Thesesuggestersareconstructedusingsomeadditionalsuggesters:SuggestGrasps,SuggestPosesandSuggestPathsTo.SuggestGrasps(O):ndsgraspsforO(gripperposesrelativetoO)withsufcientoverlapofthengersandanavailableapproachcongurationoftherobot.SuggestPoses(O;R;Taboos):ndsasetofposesforOwhereitiscompletelyinsideregionR,thereisnocollisionwithtabooregions,andthereissomevalidgrasp(asperSuggestGrasps)fortheobjectinthatpose.Theimplementationgeneratesposesintheregionanddiscardthosethatfailthatgraspingaccessibilitytests.SuggestPathsTo(O;R):ndspathsforOfromtherobot'shomeposetosomeposewithinregionR(aspersuggestPoses).Amotionplannerlazilybuildsa4-dofvisibility-graph;x;ytranslationconstraintsarerepresentedasC-spacepolygonsfordiscreterangesofzand.Linksinthevisibilitygraphrepresenteitherpurex;ytranslation,zoffsetoroffset.Intheexamplesinthispaper,itwasconstrainedtodotranslationonlyandtoreturnasinglepath.SuggestParking(O;Taboos;start):ndan“outoftheway”locationforOthatdoesnotoverlapanyoftheregionsinTaboos.TheimplementationcurrentlyissimplySuggestPosesinsomedesignatedparkingregions;thepark-ingplacesarenotpre-specied;theyaresuggestedusingsomecriteriathattendstopackthemneartheedgesofthespeciedregion.Theactualmotionplanninginthebasedomaincanbedonebyanymotionplannerthatcanplanpathsbetweenspeciedrobotcongurations.OurimplementationusesanRRT-basedplanner.VI.CORRECTNESSBecauseHPNisacombinedplanningandexecutionsystem,thestandardnotionsofcorrectnessandcompletenessfromplanningdonotapplydirectly.Ourcorrectnesscriterionisthat,ifagoalstatewasreachablefromthestartingstateundersomesequenceofoperations,thatHPNwilleventuallycausethesystemtoreachagoalstate.Ifwewereinadiscretedomain,usingeveryinstantiationoftheoperatorschemasanddidnotintroduceanyhierarchi-calplanninglevels,thenitisclearthattheregressionplannerwouldproduceacorrectplanifoneexistsanditwouldbeexecutedstepbystep.So,weneedtoexaminetheeffectsofhierarchyandofoperatingininnitedomainsontheabilityofHPNtoachievefeasiblegoals.HierarchyGivenahierarchyofoperatordescriptionsforeachtargetuent,inanitedomain,wecanconstructanoverallhierarchyofplanningdomaindescriptions(PDDs),asfollows.LetFbethesetofallgroundinstancesofalluentsthatcanbethetargetofanoperatorinanyofthePDDs,andletHbeamappingfromFintointegersselectingthelevelofabstractionofthatuent'soperator.Wespecifyapartialorderingonabstractionlevelsasfollows:H1�H2,thatis,H1ismoreconcretethanH2,iff(1)foralluentsf2F,H1(f)H2(f)and(2)thereexistsauentf2FforwhichH1(f)�H2(f).WedeneHtobetheabstractionlevelthatmapseachuenttoitscompletelyconcreteoperatorandH0tobetheabstractionlevelthatmapseachuenttoitsmostabstractoperator.Astateshasstaticconnectivityinadomain,ifallstatess1thatarereachablefromsarealsoreachablefromanys2thatisreachablefroms.Thatis,anychoiceofactionisultimately'reversible'inthesensethatitcanbeundonethroughasequenceofactions.Adomainhasstaticconnectivityifallofitsstatesdo.