Integrated Task and Motion Planning (TAMP) in robotics

In the earlier publish, we launched job planning in robotics. This area broadly entails a set of planning strategies which might be domain-independent: That is, we will design a site which describes the world at some (usually excessive) degree of abstraction, utilizing some modeling language like PDDL. However, the planning algorithms themselves will be utilized to any area that may be modeled in that language, and critically, to resolve any legitimate drawback specification inside that area.

The key takeaway of the final publish was that job planning is finally search. These search issues are sometimes difficult and develop exponentially with the scale of the issue, so it’s no shock that job planning is usually symbolic: There are comparatively few doable actions to select from, with a comparatively small set of finite parameters. Otherwise, search is prohibitively costly even within the face of intelligent algorithms and heuristics.

Bridging the hole between this summary planning area and the true world, which we deeply care about in robotics, is tough. In our instance of a cell robotic navigating a family setting, this would possibly look as follows: Given that we all know two rooms are related, a plan that takes our robotic from room A to room B is assured to execute efficiently. Of course, this isn’t essentially true. We would possibly provide you with a superbly legitimate summary plan, however then fail to generate a dynamically possible movement plan by a slender hallway, or fail to execute a superbly legitimate movement plan on our actual robotic {hardware}.

This is the place Task and Motion Planning (TAMP) is available in. What if our planner spends extra effort deliberating about extra concrete features of a plan earlier than executing it? This presents a key tradeoff in additional up-front computation, however a decrease threat of failing at runtime. In this publish we’ll discover a number of issues that differentiate TAMP from “plain” job planning, and dive into some detailed examples with the pyrobosim and PDDLStream software program instruments.

Some motivating examples

Before we formalize TAMP additional, let’s take into account some tough examples you would possibly encounter with purely symbolic planning in robotics functions.

In this primary instance, our aim is to choose up an apple. In purely symbolic planning the place all actions have the identical value, there is no such thing as a distinction in navigating to table0 and table1, each of which have apples. However, you’ll discover that table0 is in an unreachable location. Furthermore, if we determine to embed navigation actions with a heuristic value akin to straight-line distance from the start line, our heuristic under will choose table0 as a result of it’s nearer to the robotic’s beginning place than table1.

It wouldn’t be till we attempt to refine our plan — for instance, utilizing a movement planner to seek for a legitimate path to table0 within the unreachable room — that we might fail, need to replace our planning area to one way or the other flag that main_room and unreachable are disconnected, after which replan with this new info.

Pathological job planning instance for goal-directed navigation.
Both table0 and table1 can result in fixing the aim specification of holding an apple, however table0 is totally unreachable.

In this second instance, we wish to place a banana on a desk. As with the earlier instance, we might select to position this object on both desk0 or desk1. However, within the absence of extra info — and particularly if we proceed to deal with close by places as decrease value — we could plan to position banana0 on desk0 and fail to execute at runtime due to the opposite obstacles.

Here, some different options would come with inserting banana0 on desk1, or shifting one of many different objects (water0 or water1) out of the best way to allow banana0 to suit on desk0. Either manner, we’d like some notion of collision checking to allow our planner to get rid of the seemingly optimum, however in apply infeasible, plan of merely inserting the thing on desk0.

Pathological job planning instance for object manipulation.
Placing banana0 on both desk0 or desk1 will fulfill the aim, however desk0 has different objects that will result in collisions. So, banana0 should both positioned on desk1, or the objects should be rearranged and/or moved elsewhere to permit banana0 to suit on desk0.

In each circumstances, what we’re lacking from our purely symbolic job planner is the power to contemplate the feasibility of summary actions earlier than spitting out a plan and hoping for the perfect. Specifically for embodied brokers akin to robots, which transfer within the bodily world, symbolic plans should be made concrete by movement planning. As seen in these two examples, what if our job planner additionally required the existence of a selected path to maneuver between two places, or a selected pose for putting objects in a cluttered area?

What is Task and Motion Planning?

In our examples, the core problem is that if our job planning area is simply too summary, a seemingly legitimate plan is prone to fail down the road once we name a very decoupled movement planner to strive execute some portion of that plan. So, job and movement planning is precisely as its identify states — collectively fascinated by duties and movement in a single planner. As Garrett et al. put it of their 2020 survey paper, “TAMP actually lies between discrete “high-level” job planning and steady “low-level” movement planning”.

However, there’s no free lunch. When contemplating all of the wonderful particulars up entrance in deliberative planning, search turns into costly in a short time. In symbolic planning, an motion could have a discrete, finite listing of doable objectives (let’s say someplace round 5-10), so it might be cheap to exhaustively search over these and discover the one parameter that’s optimum in keeping with our mannequin. When we begin fascinated by detailed movement plans which have a steady parameter area spanning infinite doable options, this turns into intractable. So, a number of approaches to TAMP will apply sampling-based strategies to make planning work in steady motion areas.

Another manner to make sure TAMP is sensible is to leverage hierarchy. One widespread method for breaking down symbolic planning into manageable items is Hierarchical Task Networks (HTNs). In these 2012 lecture slides, Nilufer Onder mentions “It would be a waste of time to construct plans from individual operators. Using the built-in hierarchy helps escape from exponential explosion.” An instance of hierarchical planning is proven within the diagram under. Using this diagram, you may discover the advantages of hierarchy; for instance, this planner would by no means need to even take into account tips on how to open a door if the summary plan didn’t require happening the hallway.

An instance of hierarchical planning for a robotic, the place high-level, or summary, plans for a robotic may very well be refined into lower-level, or concrete, actions.
Source: Automated Planning and Acting (2016)

Hierarchical planning is nice in that it helps prune infeasible plans earlier than spending time producing detailed, low-level plans. However, on this area the legendary downward refinement property is usually cited. To immediately quote the 1991 paper by Bacchus and Yang, this property states that “given that a concrete-level solution exists, every abstract solution can be refined to a concrete-level solution without backtracking across abstract levels”. This isn’t at all times (and I might argue hardly ever) achievable in robotics, so backtracking in hierarchical planning is essentially unavoidable.

To this finish, one other technique behind TAMP has to do with dedication in sampling parameters throughout search. In the literature, you will notice many equal phrases thrown round, however I discover the principle distinction is between the next methods:

• Early-commitment (or binding) methods will pattern motion parameters from steady area earlier than search, successfully changing the issue to a purely discrete job planning drawback.
• Least-commitment (or optimistic) methods will as an alternative provide you with a purely symbolic plan skeleton. If that skeleton is possible, then the required parameter placeholders are crammed by sampling.

Flowcharts representing two excessive kinds of sampling-based TAMP.
*H-CSP = hybrid constraint satisfaction drawback
Source: Garrett et al. (2020), Integrated Task and Motion Planning

Both methods have benefits and drawbacks, and in apply fashionable TAMP strategies will mix them not directly that works for the varieties of planning domains and issues being thought of. Also, observe that within the diagram above each methods have a loop again to the start when an answer isn’t discovered; so backtracking stays an unavoidable a part of planning.

One key paper that demonstrated the stability of symbolic search and sampling was Sampling-based Motion and Symbolic Action Planner (SMAP) by Plaku and Hager in 2010. Around the identical time, in 2011, Leslie Kaelbling and Tomás Lozano-Pérez offered Hierarchical Planning within the Now (HPN), which mixed hierarchy and sampling-based strategies for TAMP. However, the authors themselves admitted the sampling half left one thing to be desired. There is a good quote on this paper which foreshadows a number of the different work that will come out of their lab:

“Because our domains are infinite, we cannot consider all instantiations of the operations. Our current implementation of suggesters only considers a small number of possible instantiations of the operations. We could recover the relatively weak properties of probabilistic completeness by having the suggesters be generators of an infinite stream of samples, and managing the search as a non-deterministic program over those streams.”

– Leslie pack kaelbling and Tomás Lozano-Pérez (2011), Hierarchical planning within the now.

Directly following this quote is the work their pupil Caelan Garrett took on — first within the creation of STRIPStream in 2017 after which PDDLStream in 2018. The astute reader may have seen that PDDLStream is the precise software program utilized in these weblog posts, so take this “literature review” with this bias in thoughts, and hold studying if you wish to be taught extra about TAMP with this particular software.

If you wish to know extra about TAMP normally, I’ll refer you to 2 latest survey papers that I discovered helpful:

Mobile robotic instance, revisited

To illustrate the advantages of built-in TAMP, we’ll proceed the identical cell robotics instance from the earlier publish. In this drawback,

• The robotic’s aim is to position the apple on the desk.
• Navigation now requires developing with a aim pose (which is a steady parameter), as effectively the precise path from begin to aim. For this instance, we’re utilizing a Rapidly-exploring Random Tree (RRT), however you may swap for every other path-finding algorithm.
• Placing an object now requires sampling a legitimate pose that’s inside the location floor polygon and doesn’t collide with different objects on that floor.

As you learn the next listing explaining this drawback, be sure you scroll by the slideshow under to get a visible illustration.

STEP 1: Looking on the state of the world, you may see how a purely symbolic job planner would output a comparatively easy plan: decide the apple, transfer to the desk, and place the apple on the desk. In the context of TAMP, this now represents a plan skeleton which a number of parameters which might be but to be crammed — particularly,

• ?pt is the pose of the robotic when navigating to the desk
• ?path is the precise output of our movement planner to get to ?pt
• ?pa-1 is the brand new pose of the apple when positioned on the desk (which follows from its preliminary pose ?pa-0)

STEP 2: To make the issue slightly easier, we made it such that each location has a discrete, finite set of doable navigation places equivalent to the perimeters of its polygon. So trying on the desk location, you see there are 4 doable navigation poses pt-T, pt-B, pt-L, and pt-R equivalent to the highest, backside, left, and proper sides, respectively. Since this set of places is comparatively small, we will pattern these parameters up entrance (or eagerly) at the beginning of search.

STEP 3: Our transfer motion can now have totally different instantiations for the aim pose ?pt which might be enumerated throughout search. This is in distinction with the ?path argument, which have to be sampled by calling our RRT planner. We don’t wish to do that eagerly as a result of the area of paths is steady, so we choose to defer sampling of this parameter. If our motion has a value related to the size of a path, we might think about that the lowest-cost motion can be to navigate to the left aspect of the desk (pt-L), and a few randomly sampled path (path42) could describe how we get there.

STEP 4: Next comes the place motion, which now should embody a legitimate collision-free pose for the apple on the desk. Because of how we arrange our drawback, our robotic can’t discover a legitimate placement pose when approaching from the left aspect of the desk. So, we should backtrack.

STEP 5: After backtracking, we have to discover another navigation pose for the desk (?pt). Given the environment, the one different possible location is the underside aspect of the desk (pt-b), because the partitions block the robotic from the highest and proper sides and it will be unimaginable to discover a legitimate path with our RRT. However, when the robotic is on the backside aspect of the desk, it might additionally pattern a legitimate placement pose! In our instance, the placeholder ?pa-1 is subsequently glad with some randomly sampled pose pa29.

STEP 6: … And there you have got it! A sound plan that defines a sequence of symbolic actions (decide, transfer, place) together with the required navigation pose, path to that pose, and placement location for the apple. It’s not optimum, however it’s probabilistically full!

(1/6) By being optimistic about all the continual parameters associated to movement, we will attain a possible aim state with relative ease.

(2/6) Since the navigation poses across the desk and the desk are finite, we will pattern them eagerly; that’s, we enumerate all choices up entrance in planning.

(3/6) Once we decide to a navigation pose across the desk, we will proceed filling in our plan by sampling a possible trajectory from the robotic’s present pose to the goal pose on the desk.

(4/6) Next, we have to pattern a placement pose for the apple. Suppose on this case we fail to pattern a collision-free answer based mostly on the robotic’s present location.

(5/6) This means we have to backtrack and take into account a unique navigation pose, thereby a unique movement plan to this new pose.

(6/6) From this new pose, although the trajectory is longer and subsequently higher-cost, we will pattern a legitimate placement pose for the apple and at last full our job and movement plan.

Now, suppose we alter the environment such that we will solely strategy the desk from the left aspect, so there is no such thing as a option to immediately discover a legitimate placement pose for the apple. Using the identical planner, we must always ultimately converge on a job and movement plan that rearranges the objects world — that’s, it requires shifting one of many different objects on the desk to make room for the apple.

Implementing TAMP with PDDLStream

We will now revisit our pathological examples from the start of this publish. To do that, we’ll use PDDLStream for planning and pyrobosim as a easy simulation platform. For fast background on PDDLStream, you could check with this video.

The key thought behind PDDLStream is that it extends PDDL with a notion of streams (bear in mind the sooner quote from the Hierarchical Planning within the Now paper?). Streams are generic, user-defined Python capabilities that pattern steady parameters akin to a legitimate pattern certifies that stream and supplies any obligatory predicates that (often) act as preconditions for actions. Also, PDDLStream has an adaptive method that balances exploration (trying to find discrete job plans) vs. exploitation (sampling to fill in steady parameters).

Goal-directed navigation

We can use PDDLStream to reinforce our transfer motion such that it contains metric particulars in regards to the world. As we noticed in our illustrative instance, we now should issue within the begin and aim pose of our robotic, in addition to a concrete path between these poses.

As extra preconditions for this motion, we should make sure that:

• The navigation pose is legitimate given the goal location (NavPose)
• There have to be a legitimate path from the begin to aim pose (Motion)

Additionally, we’re in a position to now use extra life like prices for our motion by calculating the precise size of our path produced by the RRT! The separate file describing the streams for this motion could look as follows. Here, the s-navpose stream certifies the NavPose predicate and the s-motion stream certifies the Motion predicate.

The Python implementations for these capabilities would then look one thing like this. Notice that the get_nav_poses perform returns a finite set of poses, so the output is a straightforward Python listing. On the opposite hand, sample_motion can repeatedly spit out paths from our RRT, and it carried out as a generator:

Putting this new area and streams collectively, we will resolve our first pathological instance from the introduction. In the plan under, the robotic will compute a path to the farther away, however reachable room to choose up an apple and fulfill the aim.

Object manipulation

Similarly, we will prolong our place motion to now embody the precise poses of objects on this planet. Specifically,

• The ?placepose argument defines the goal pose of the thing.
• The Placeable predicate is licensed by a s-place stream.
• The IsCollisionFree predicate is definitely a derived predicate that checks particular person collisions between the goal object and all different objects at that location.
• Each particular person collision test is set by the CollisionFree predicate, which is licensed by a t-collision-free steam.

The Python implementation for sampling placement poses and checking for collisions could look as follows. Here, sample_place_pose is our generator for placement poses, whereas test_collision_free is a straightforward Boolean (true/false) test.

By increasing our area to cause in regards to the feasibility of object placement, we will equally resolve the second pathological instance from the introduction. In the primary video, we have now another location desk1 the place the robotic can place the banana and fulfill the aim.

In the second video, we take away the choice desk1. The similar job and movement planner then produces an answer that entails selecting up one of many objects on desk0 to make room to later place the banana there.

You can think about extending this to a extra life like system — that’s, one that isn’t some extent robotic and has an precise manipulator — that equally checks the feasibility of a movement plan for choosing and inserting objects. While it wasn’t the principle focus of the work, our Active Learning of Abstract Plan Feasibility work did precisely this with PDDLStream. Specifically, we used RRTs to pattern configuration-space paths for a Franka Emika Panda arm and doing collision-checking utilizing a surrogate mannequin in PyBullet!

Conclusion

In this publish we launched the overall idea of job and movement planning (TAMP). In concept, it’s nice to deliberate extra — that’s, actually suppose extra in regards to the feasibility of plans all the way down to the metric degree — however with that comes extra planning complexity. However, this may repay in that it reduces the danger of failing in the course of executing a plan and having to cease and replan.

We launched 3 common rules that may make TAMP work in apply:

• Hierarchy, to find out the feasibility of summary plans earlier than planning at a decrease degree of refinement.
• Continuous parameter areas, and strategies like sampling to make this tractable.
• Least-commitment methods, to provide you with symbolic plan skeletons earlier than spending time with costly sampling of parameters.

We then dug into PDDLStream as one software for TAMP, which doesn’t do a lot in the best way of hierarchy, however actually tackles steady parameter areas and least-commitment methods for parameter binding. We went by a number of examples utilizing pyrobosim, however you may entry the total set of examples within the pyrobosim documentation for TAMP.

The PDDLStream repository has many extra examples which you could try. And, in fact, there are various different job and movement planners on the market that target various things — akin to hierarchy with out steady parameters, or factoring in different widespread aims akin to temporal features and useful resource consumption.

Hope you have got loved these posts! If the instruments proven right here offer you any cool concepts, I might love to listen to about them, so be happy to succeed in out.

You can learn the unique article at Roboticseabass.com.