Abstract
Task and motion planning (TAMP) is a key research field for robotic manipulation tasks. The goal of TAMP is to generate motionfeasible task plan automatically. Existing methods for checking motion feasibility of task plan skeletons have some limitations of semanticfree pose candidate sampling, weak search heuristics, and early value commitment. In order to overcome these limitations, we propose a novel constraint satisfaction framework for checking motion feasibility of task plan skeletons. Our framework provides (1) a semantic pose candidate sampling method, (2) novel variable and constraint ordering heuristics based on intra and interaction dependencies in a task plan skeleton, and (3) an efficient search strategy using constraint propagation. Based upon these techniques, our framework can improve the efficiency of motion feasibility checking for TAMP. From experiments using the humanoid robot PR2, we show that the motion feasibility checking in our framework is 1.4x to 6.0x faster than previous ones.
1. Introduction
In recent years, Artificial Intelligence (AI) has become an increasingly common presence in robotic solutions, introducing flexibility and learning capabilities in previously rigid applications. In this study, we address the issues in applying AI constraint satisfaction problem (CSP) solving techniques to task and motion planning (TAMP) [1–37], which is a key research field of robotic manipulation tasks. A typical TAMP involves the combination of task planning [38–40] that generates a sequence of actions for satisfying a goal condition in the current state based on a symbolic abstract action model and motion planning [41–43] that checks the motion feasibility for determining if the motions can be executed in a physical space. The ultimate goal of TAMP is to combine these to generate a motionfeasible task plan.
Task planning determines the logical order of actions. It knows symbolic knowledge such as type of action, parameters, preconditions, and effects. However, in task planning, the goal pose (or configuration), path (or trajectory), etc., to execute each action are unknown. This disadvantage can be supplemented through motion planning. While abstract symbolic knowledge is unknown in motion planning, it can generate collisionfree paths for executing an action in a physical space. To combine the two planning methods, which have different advantages and disadvantages, an intermediate (interface) layer is required. Figure 1 shows an example of combining task and motion planning to generate a motionfeasible task plan. The left side of Figure 1 shows the task layer including the task planning process.
The right side of Figure 1 shows the motion layer including the motion planning process. With the help of the task layer, the interface layer generates a task plan, that is, a task plan skeleton that includes unbound pose parameters. Once the task plan skeleton is generated, the interface layer generates candidates of pose parameters of each action. As motion planning cannot generate the goal pose on its own, the interface layer must play a role in generating candidates of pose parameters. The interface layer then checks if there is a collisionfree path between candidates of neighboring pose parameters with the help of the motion layer. If all of collisionfree paths exist, then the candidates are assigned to the pose parameters of the task plan skeleton to generate a motionfeasible task plan. Otherwise, it creates a new task planning problem from motionrelated errors (e.g., obstacles), and repeats the same interfacing process.
This paper focuses on checking the motion feasibility of the task plan skeleton in the interface layer. Motion feasibility checking involves finding motionfeasible values of unbound pose parameters included in the task plan skeleton. This is a challenging problem with a very complex search space. It expands the motion planning problem that finds the collisionfree path of a single behavior to the problem of validating the pose candidates and collisionfree paths of multiple actions. To solve this problem effectively, the followings are required: (1) theoretical modelling of the search problem to find motionfeasible values of pose parameters, (2) a method for generating candidates of the value of pose parameters in continuous space, and (3) search strategies and heuristics (e.g., using constraints that the pose parameters must satisfy).
The previous study of LozanoPérez and Kaelbling [18] attempted to model and to solve the motion feasibility checking problem as a traditional CSP (Constraint Satisfaction Problem) [44, 45]. Garrett et al. [37] attempted to model and to solve the motion feasibility checking problem as onthefly CSP. Lagriffoul et al. [17] attempted to model and to solve the motion feasibility checking problem as a general search tree. These studies have limitations, including generating pose candidates with a high probability of failure due to simple sampling methods, or developing search strategies based on only generalpurpose heuristics that is not suitable for motion feasibility checking. Furthermore, they attempted to determine values in advance that must be redetermined at the time of execution, such as IK (Inverse Kinematics) and collisionfree path with high variability. It complicates the search problem and makes planning and execution inefficient in the long run.
This paper proposes an efficient constraint satisfaction framework to address the motion feasibility checking problem. Motion feasibility checking is the problem that finds a valid (or motionfeasible) value of pose parameters. The valid value means that it satisfies constraints such as kinematics and collisionfree path. In our framework, the motion feasibility checking is modelled as a constraint satisfaction problem (CSP). The CSP formulation has the advantage that the various types of constraints can be represented equally in a concise form. In addition, effective generalpurpose or specialpurpose search strategies and heuristics can be used to solve both discrete and continuous constraint satisfaction problems. (1) Our framework proposes a method of generating pose candidates guided by the semantics of unbound pose parameters and already bound task parameters. This method reduces the search space substantially. Meanwhile, actions belonging to the task plan skeleton have interaction dependencies because of logical order. In addition, parameters and preconditions within an action have intraaction dependencies. (2) Based on these inter and intraaction dependencies, novel, variable, and constraint ordering heuristics are proposed in our framework. (3) Moreover, our framework employs an efficient search strategy with constraint propagation [44] to detect failure early or speed up the search substantially. To verify the practical applicability and performance of the proposed framework, this study performs various experiments using humanoid robots that can perform mobile manipulation.
The rest of the paper is organized as follows. Section 2 introduces the related works in the field of TAMP. Section 3 provides the problem statement. In Section 4, the proposed framework with CSP modelling and heuristic search methods is presented in detail. In Section 5, experimental results are reported. Finally, Section 6 summarizes our work and discusses some limitations and future works.
2. State of Art
We categorize previous works according to their motion feasibility checking methods, as shown in Table 1. There are two different methods for checking motion feasibility. The eager method [1–14] alternately performs task planning and motionfeasibility checking stepbystep. On the contrary, the lazy method [15–37] postpones its motionfeasibility checking until a complete skeleton of task plan is built. The eager method has the advantage of being able to identify in advance future motionunfeasible actions that the task planner cannot identify during the task planning process. However, the cost of generating the motion plan is high because motion planning is continuously interleaved during task planning. Although the lazy method cannot identify in advance the motion feasibility of the actions during task planning, it limits the search space of the motion planning to a small range since the task plan to achieve the goal condition is already defined. In addition, it examines different knowledge contained in the task plan skeleton and applies a wide range of search strategies and heuristics. This paper utilizes the lazy method.
Especially, LozanoPérez and Kaelbling [18], Garrett et al. [37], and Lagriffoul et al. [17] are closely related to our study, because they focus on motion feasibility checking of task plan skeletons using constraint satisfaction methods. First, LozanoPérez and Kaelbling [18] attempted to model the motion feasibility checking problem as a traditional CSP. In [18], the depthfirst backtracking search algorithm and constraint propagation considering the dependency between constraints are used. However, [18] used only generalpurpose variable ordering heuristics such as MRV (Minimum Remaining Value), which did not consider some dependencies between actions and in an action. Furthermore, [18] generated pose candidates with high probability of failure and dealt with only the parameters and constraints related to the manipulation actions.
Next, Garrett et al. [37] attempted to model the motion feasibility checking problem as onthefly CSP. In particular, [37] proposed a conditional sampling method based on the constraint network. However, the CSP modelled by [37] had some variables that cause high cost of motion planning, such as IK and trajectories. It makes the search problem more complex. The method of [37] is also less scalable because it only considers generalpurpose CSP heuristics.
Finally, Lagriffoul et al. [17] attempted to model the motion feasibility checking problem as a general search tree. The nodes of the tree represent pose parameters, and the edges represent collisionfree trajectories. Based on the depthfirst backtracking search, it finds the values of pose parameters with verified validity for the trajectory. In particular, [17] attempted to reduce the number of backtracks by propagating linear constraints before visiting the next nodes. It is the same as forward checking in CSP. However, search tree does not represent all constraints in a unified form, and its simple sampling method generates many samples with a high probability of failure. In addition, [17] dealt only with the parameters and constraints associated with manipulation actions.
In order to overcome the limitations of these existing studies, our framework provides (1) a semantic pose candidate sampling method, (2) novel variable and constraint ordering heuristics based on intra and interaction dependencies in a task plan skeleton, and (3) an efficient search strategy using constraint propagation. Based upon these techniques, our framework can improve the efficiency of motion feasibility checking for TAMP.
In addition, there are several recent notable works on TAMP (Task and Motion Planning). The authors of [32] proposed a TAMP framework using a topk skeleton planner to produce diverse skeletons, guaranteeing that no better solution exists under a current domain description. Moreover, the framework uses a MonteCarlo Tree Search (MCTS) to solve this stochastic decisionmaking problem over skeletons and concrete bindings of the action parameters. The authors of [33] proposed a novel online planning and execution system to solve a TAMP problem as a hybrid partially observable Markov decision process (POMDP) and use past plans to constrain the structure of solutions for the current planning problem. On the other hand, both [34, 35] addressed multiagent or multirobot TAMP problems. The authors of [34] dealt with a new problem of motion planning feasibility checking for taskagent assignment to perform complex tasks using multiarm mobile manipulators. The authors of [35] presented a multirobot integrated task and motion planning method capable of handling sequential subtasks dependencies within multiple decomposable tasks. Interestingly, [36] proposed a TAMP method for efficient and safe autonomous urban driving, different from robotic manipulation tasks.
3. Problem Statement
We propose an efficient constraint satisfaction framework to check the motion feasibility of the task plan skeleton. We assume that a task plan skeleton has already been generated. Then, we focus on checking its motion feasibility.
Figure 2 illustrates an example environment including a humanoid robot with two arms (e.g., PR2) to manipulate objects on tables. In Figure 2, PR2 must move the object from one table to the other.
We remark the task plan skeleton as follows:
Remark 1. (task plan skeleton). A task plan skeleton is a sequence of actions <a0, …, ak> following the states transition <s0, …, sk+1>, where a∈A is an action, s0 the initial state, and sk+1 the goal state, which satisfies the goal condition. Meanwhile, each action a has pose references.
The pose references are discrete identifiers for representing the values of the poses still undetermined in the task planning phase. The pose references are regarded to have values that satisfy the preconditions of the actions. To generate the task plan skeleton, the pose references are temporarily bound to the pose parameters.
Table 2 shows an example of a task plan skeleton generated from the task environment in Figure 2. This task plan skeleton consists of a sequence of actions in which the robot moves object03 to table 02.
The actions that comprise the task plan skeleton are specified according to the schema below.
Remark 2. (action schema). Action schema is a tuple (α (Pt, Pp), pre (Pt, Pp, V), eff (Pt, Pp, V)), where α is an action symbol; Pt is a set of tasklevel parameters such as object, robot, etc.; Pp is a set of pose parameters; pre (Pt, Pp, V) is a conjunction of predicates representing preconditions of action (where V are additional variables, V ≠ Pt, Pp); and eff (Pt, Pp, V) is a conjunction of predicates representing effects of action.
Table 3 shows the specifications of the actions for navigation. In the parameters for each action, there are numerical parameters that were not in the previous action specification. For example, baseLoc0 and baseLoc1 are pose parameters, where baseLoc0 is the robot’s current location and baseLoc1 the goal location. During the generation of the task plan skeleton, these parameters will be bound to pose references. Based on the preconditions of the action, the pose parameters must satisfy the state conditions at the motion level. For example, the baseLoc1 parameter of MoveBasePickUp must satisfy state conditions such as reachableBasePose and validBasePath. Section 4 details the specific meanings of these state conditions.
Table 4 shows the specifications of the actions for manipulation. First, PickUpGeneral is the action of moving a hand to grasp an object and then returning the hand back to its initial pose. This action contains many motions, thus requiring a relatively large number of pose parameters. This action includes the initial hand pose (handPose0), the pregrasping hand pose (handPose1), the grasping hand pose (handPose2), and the postgrasping hand pose (handPose3). PutDownGeneral is the action of putting the grasped object on a support plane such as a table and returning the hand to the initial pose. This action has pose parameters similar to the PickUpGeneral action. However, the PickUpGeneral action includes a pose parameter for the placement position of the object. Section 4 details the specific meanings of these state conditions.
Under the assumptions described above, we summarize the main problem for checking the motion feasibility of the task plan skeleton as follows.
Remark 3. (motion feasibility checking). Given a task plan skeleton S, the problem is to find a sequence of valid values <, …, > of pose references in S. The valid values <, …, > satisfy preconditions of corresponding action.
We attempt to model and solve this motion feasibility checking problem by converting it to a CSP [32, 33]. CSP refers to the problem of finding a value that satisfies a plurality of constraints within a domain. The valid values of the undetermined pose parameters must satisfy the state conditions presented in the taskplanning problem so that they can be fully formulated as a CSP. As poses exist in a continuous space, discrete domains are created to find a solution in a reasonable amount of time. Thus, the CSP considered in this paper is a discrete CSP. Now, we summarize subproblems as follows:
Remark 4. (constructing discrete CSP). Given a task plan skeleton S, the problem is to construct a discrete CSP csp = (V, D, C) from the task plan skeleton, where V is a set of variables, D is a set of the respective domains of values, and C is a set of constraints.
Remark 5. (solving CSP). Given a discrete CSP csp, the problem is to search valid values of each variable in V.
4. Proposed Solution
The task plan skeleton contains unbound pose references. Values for the pose references must satisfy the motionrelated constraints, such as IK and the collisionfree path. If any value satisfying all constraints exists, then we can consider that the task plan skeleton is motion feasible. This is a constraintbased search problem. So, we model the problem for checking the motion feasibility as a CSP.
Figure 3 describes the process of motion feasibility checking. First, it generates CSP statements from a task plan skeleton. Second, it reduces the size of domain through preprocessing before solving CSP. Last, it finds a solution and decides if the task plan skeleton is motion feasible or not.
4.1. Formulating Constraint Satisfaction Problem from Plan Skeleton
4.1.1. Variables
First, we have to formulate the motion feasibility checking problem into a constraint satisfaction problem (CSP). Usually, goal pose, grasp, IK, and path were modelled as CSP variables in a way a similar to that in [17, 18, 37]. But, we only model the goal pose to the CSP variable, that is, modelling the path to the CSP variable. The reason for this is as follows: (1) If too many variables are modelled, it increases the complexity of search. In addition, the domain of variable (for path especially) is very difficult and expensive. (2) To bind the value of anything other than the pose is not required necessarily in the planning phase. Considering changes of the environment during planning time, even if the IK, paths, etc., are determined at the planning phase, rechecking is required at the execution phase. It is appropriate that these are assigned once at the execution phase.
We categorize the poses into three types. The types include position of mobile base B, pose of hand H, and position of object O. The variables are extracted from the pose references of the actions the task plan skeleton. Table 2 is a mapping table of the abovementioned variables. As shown in Table 5, the pose references bound to each action are mapped as variables of the CSP. However, since the initial poses of mobile base, hand, and object are constants that can be read directly from the environment, these pose references are not mapped to variables of the CSP. In addition, a pose reference that has already been mapped as B_{reach} is not mapped in duplicate.
4.1.2. Domains
Each variable is represented by a 4 × 4 homogeneous matrix in continuous space. For execution, a discretized deterministic value must be bound to each variable. For this reason, we generate discrete domains based on the sampling guided by the semantic of the task plan skeleton. To generate the discrete domains, the geometric constants are retrieved from the value of the bounded parameter such as mobile base, hand, object, and support plane. For example, referring the value of bounded parameters, we can know that the domain of B_{reach} should be near the object and the orientation of B_{reach} should be towards the object. This can be represented by the following:
where r_{0} is a matrix representing an axis angle , r_{1} is a matrix representing a quaternion , t_{0} is a matrix representing a pose , and r_{2} is a matrix representing an axis angle . The details of the equations that derive the matrix from the axis angle, quaternion, etc., are provided in the appendix.
Equation (1) is to calculate the relative position and orientation of the mobile base relative to the coordinate system of the object. This equation has two parameters: d and θ, where d indicates the distance between the center of the robot and the center of the object, whereas θ indicates the direction of the robot towards the object. d can be appropriately determined in consideration of the arm length. θ is calculated by equation (2):
If the number of d is m, then the number of domains is mn.
Figure 4 shows the area of the domain of B_{reach}. In Figure 4(a), the circular dotted line that maintains the distance d with the object is the range of domain. Figure 4(b) shows an example of a domain generated when m is 1 and n is 4. The domain of B_{place} is generated in the similar way as the domain of B_{reach}. However, it calculates the distance d on the basis of the center of the table.
(a)
(b)
Also, we can know that the domain of H_{grasp} should be located at a location very close to the object and the orientation of B_{reach} should be towards the object. This can be represented in an equation (3) as follows:
where r_{0} is a matrix representing a quaternion , r_{1} is a matrix representing an axis angle , t_{0} is a matrix representing a pose , r_{2} is a matrix representing an axis angle , and t_{1} is a matrix representing a pose .
This equation is to calculate the position and orientation of the hand relative to the coordinate system of the object. This equation has three parameters: d, θ, and h. d is the distance between the center of the hand and the center of the object, h is the height of the hand from the bottom of the object, and θ is the direction of the hand towards the object. d can be appropriately determined in consideration of the finger length. h may be determined in consideration of the volume of the hand, etc. If the number of d is m and the number of h is k, then the number of domains is mkn.
Figure 5 shows the area of the domain of H_{grasp}. In Figure 5(a), the circular dotted line that maintains the distance with the object d and the height h is the domain. Figure 5(b) shows an example of a domain generated when m is 1, k is 1, and n is 6. The domains of H_{pre} and H_{post} are sampled similar to the domain of H_{grasp}. However, d for the domain of H_{pre} is farther than d for the domain of H_{grasp}, and h for the domain of H_{post} is higher than h for the domain of H_{grasp}.
(a)
(b)
In the case of O_{place}, we can know that the domain of O_{place} should be located on the support plane. To prevent falling, the position should be slightly further inside from each corner of the support plane. When O_{place} is a matrix representing a pose , x, y, and z are constrained as below.
In equation (4), the support plane P is assumed to be rectangular. This equation has parameters x, y, z, and p. x and y show the position of the object on the support plane. z represents the height of the object. p represents the space inward from the edge of the support plane, that is, the padding. If the number of x is m, and if the number of y is n, then the number of domains is mn.
Figure 6 shows the area of the domain of O_{place}. The gray area on the table is the area of the domain. The gray area represents the remaining area of the support plane excluding padding. Figure 6(b) shows an example of a domain generated at linear intervals when m is 1 and n is 4.
(a)
(b)
4.1.3. Constraints
The constraint types are categorized into unary constraints (e.g., reachableBasePose (B, o)), binary constraints (e.g., placeableBasePose (B, O)), and nonbinary constraints (e.g., preGraspablaHandPose (H, H′, B, h, o)) according to arity in the constraints. These constraints are all extracted from the preconditions of each action in the task plan skeleton. Table 6 is a mapping table for this. For convenience, among the parameters in the constraints, constants are excluded from the variables shown.
There is an issue of how to deal with the pose of movable object to be affected by the previous action. There are some ideas for this. In LozanoPérez and Garrett, some constraints have a variable of a movable object that was affected by previous action. For example, when they assume that there is a plan skeleton for two pick and place tasks, first disjoint (…, {O1}) constraint is modelled from the first place action and then disjoint (…, {O2, O1}) constraint is modelled from the second place action although the second place action does not have a parameter of variable O1. However, this is not scalable. In Lagriffoul et al., they avoided this issue by fixing the goal pose of each objects.
In this paper, we model the constraint that only has variables that correspond to selfaction parameters. For instance, placeableHandPose constraint has only one variable for object pose regardless of the number of place actions. This constraint considers the poses of other movable objects as constraints that are loaded from the environment. This is very scalable to model the CSP from the plan skeleton. But it may assign the wrong pose (that collides with other movable objects) to a variable. To avoid this, we propose a variable ordering in section 4.3.
Meanwhile, we classify the constraints into the pose constraints and path constraints according to their semantics. The path constraints are validBasePath and validHandPath, and the remainder is the pose constraint. All constraint descriptions are summarized in a table in the appendix. The classification of constraints according to semantic is used in heuristic design.
4.2. Preprocessing
Preprocessing is performed to reduce the search space by reducing the size of domain sets. In the preprocessing step, node consistency is checked using the unary constraints. Through the node consistency, the values that satisfy the unary constraints are left and others are removed. In Table 6, the unary constraints are reachableBasePose (B) and placeableObjectPose (O).
Figure 7 shows an example of checking node consistency using reachableBasePose (Breach). As shown in Figure 7(a), when there are 4 domain values (①–④) for variable Breach, when the node consistency is checked for reachableBasePose (Breach), domain values ③ and ④ are removed as shown Figure 7(b). Domain value ③ is removed because the target object does not exist in the robot’s configuration space accessible by the robot’s arm, and domain value④ is removed because it collides with the table.
(a)
(b)
4.3. Ordering Heuristics
In the CSP, ordering heuristics (the search order of variables, the call order of constraints, etc.) are greatly influenced by the overall search time. We propose two ordering heuristics, variable ordering and constraint ordering. Variable ordering considers the interaction dependency and the intraaction dependency.
4.3.1. Variable Ordering
First, our variable ordering considers the interaction dependency. We mentioned the movable object issue of our modelling method in section 4.1.3. This variable ordering solves this issue. This takes into account the order between actions. All variables belonging to the earliest ordered actions are visited first. Then, we can get the priority of variable visits like Table 7. One thing to note is that after bounding values to all the variables of an action, the effect of this action is projected into the environment. As the effect of an action changes the environment, the variables of the next action must be visited. Then, when moving several objects from one table to another, the position of the next object can be determined considering the placement of the previously moved object.
Even if the ordering is conducted considering interaction dependency, because the manipulation actions such as the PickUpGeneral and PutDownGeneral include multiple CSP variables, ordering between the variables in these actions is not completely performed. Ordering considering the intraaction dependency between variables orders multiple variables within one action. The intraaction dependency between the variables is as follows. For the PickUpGeneral action, H_{grasp} affects H_{pre}, and H_{post}, H_{pre}, and H_{post} do not affect each other. For PutDownGeneral, the placement position O_{place} affects H_{place}, H_{low}, and H_{ret}, H_{place} affects H_{low} and H_{ret}, H_{low} and H_{ret} do not affect each other. Figure 8 denotes this as a constraint network. Based on the constraints modelled by these dependencies, this paper determines the search order of the variables using MRV, a generalpurpose CSP heuristic.
Table 8 shows the search order of the variables finally determined by this ordering. The variable ordering proposed in this paper is a mixture of generic variable ordering and specialpurpose variable ordering.
4.3.2. Constraint Ordering
As mentioned in Section 4.1.3, in addition to the general classification method of classifying constraints according to the number of parameters, this paper classifies the constraints into pose constraints and path constraints according to the semantics of the constraints. The path constraints are validBasePath and validHandPath, and the remainders are the pose constraints. This constraint classification method can be used to determine the calling order of the constraints. The calling order of the constraints is determined as first calling the pose constraint and then the path constraint. Typically, to generate a single motion plan, a valid goal pose that satisfies the constraints is first determined, and then a trajectory to the goal pose is calculated. In practice, the cost of calculating the trajectory is large enough to comprise most of the cost for checking motion feasibility. Therefore, if the trajectory is calculated for an invalid goal pose, the search time is greatly wasted. The constraint calling order has the effect of prepruning invalid pose values, thus preventing unnecessary path constraint calls.
4.4. Constraint Propagation
In the search phase, a depthfirst backtracking algorithm [44] involving constraint propagation is used to search the solution. Constraint propagation is a technique that tentatively removes the domains of other variables that violate the constraints based on the values bound to the currently visited variables during the backtracking process. Therefore, backtracking with constraint propagation can result in better performance than normal backtracking as it reduces the size of the domains to be visited. GAC (Generalized Arc Consistency) [45] is generally used to propagate nonbinary constraints fully. GAC is an extension of arc consistency [45] used to propagate constraints with more than two variables. However, full propagation of nonbinary constraints not only requires excessive costs for motion planning but also is difficult to see the reduction effect in the domains of the transitive variables. Therefore, we determined that it is more reasonable to use forward checking, which propagates the constraints only to variables neighboring the currently visited variables. In particular, when conducting forward checking, only the pose constraint check is performed to the exclusion of the path constraint check. This is because, first, it is reasonable to check the domains after first reducing them as much as possible because path constraint checking requires considerable computation time. In practice, the cost of path constraint checking accounts for most of the cost of motion feasibility checking. Second, the path constraints are dependent on the pose constraints. Thus, a valid goal pose must be determined first to enable the search for a collisionfree path to the goal pose.
The proposed framework solves the constraint satisfaction problem by using the search method shown in Algorithm 1. Algorithm 1 describes a depthfirst backtracking search that involves forward checking. In Algorithm 1, one of the domain’s values is bound to the visited variable, and then forward checking is performed on the neighboring variables of the current variables among those still unbound. As mentioned above, only the pose constraint check is performed in forward checking. The path constraint check is applied when the values are bound to the visited variables, in which constraint checking is performed with the remaining variables.

Algorithm 2 describes the constraint propagation function. This algorithm propagates pose constraints to variables that are still unbound and neighboring the currently visited variable. When propagating a pose constraint, the size of the domain of the neighboring variables may be partially reduced by the pose constraint. If all domain values violate the constraint, then this is treated as failure. The part of the domain in which values are removed is in the revise function. The revise function conducts a pose constraint check and removes all domain values that violate the pose constraints.

5. Implementation and Evaluation
5.1. Implementation
Our framework was implemented in Ubuntu 16.04, Python 2.7. The FD (Fast Downward) library [46] was used for task planning, and the TrajOpt library [47] was used for motion planning. The robot task environment was implemented using the OpenRAVE [48] simulator. The modelling and solution methods of the CSP were implemented by extending the pythonconstraints opensource library [49]. The computing environment was an Intel (R) Core (TM) i77700 CPU @ 3.60 GHz, 16 GB memory.
5.2. Evaluation
We performed experiments to evaluate the generality, scalability, efficiency, and optimality of the proposed CSP framework for TAMP. The length of the task plan skeleton was set as shown in Table 9. P1 is a task plan skeleton that takes pick and place once, and P2 is a task plan skeleton that takes pick and place twice. The size of the domain set was set as shown in Table 10. Table 10 shows the size of the domain set generated for each variable. The domain set D_{2} is twice as large as the domain set D_{1}. The sizes of the obstacles were set as shown in Table 11. The obstacles were placed at table T_{pick} where the target object is located, as well as at table T_{place} for placing the target object, at the same sizes. For the obstacle sizes, none were O_{1}, 3 were O_{2}, and 6 were O_{3}.
First, an evaluation was performed to confirm comprehensiveness of constraints which the proposed CSP framework dealt with. For this evaluation, the constraints of our framework were compared with those of previous studies. Table 12 shows the evaluation results. The constraints were largely divided into pose and path constraints, and more specifically, constraints on mobile base, hand, and object. The experimental results show that out of 16 constraints, 13 checks are possible in ours, 8 in Garrett, 7 in Lagriffoul, and 4 in LozanoPérez. Our framework showed more comprehensive coverage of constraints than existing works because it includes additional constraints on both base and hand poses for mobile manipulation tasks. On the contrary, our framework includes neither hand over constraint nor stacking constraint since it mainly focuses on pick and place tasks. However, because we focus on pick and place, it does not cover constraints such as stack and hand over.
Second, experiments were conducted to investigate the effect of the proposed preprocessing. Figure 9 shows the measurements of the search time with and without preprocessing. The horizontal axis is a problem instance with different values of the experimental parameters, and the vertical axis is the search time. The experimental results demonstrate that the search with the proposed preprocessing is faster than that without preprocessing. The set of constraints listed in Table 9 includes many unary constraints. Therefore, the proposed preprocessing was much effective to reduce the search space.
Third, experiments were conducted to analyze the performance of the proposed variable ordering method. In these experiments, our variable ordering method was compared with generic ordering methods such as MRV, DH (Degree Heuristic), and random ordering. Figure 10 shows the experimental results. The proposed variable ordering method shows the fastest search time, followed by MRV, DH, and random ordering. These results demonstrate that the proposed regiondependent ordering method considering the order between the actions has a positive effect on the search time.
Fourth, experiments were conducted to analyze effect of the proposed constraint ordering. Figure 11 shows the performance with and without the proposed constraint ordering. The experimental results demonstrate that the search time with constraint ordering is faster than the search time without constraint ordering. These results indicate that the proposed constraint ordering to preprune unnecessary path constraint checks positively impacts the search time.
Fifth, experiments were conducted to analyze effect of the proposed constraint propagation. Figure 12 shows the experimental results to compare search time of the proposed constraint propagation with those of generic constraint propagation methods such as forward checking (FC) and arc consistency (AC). The experimental results demonstrate that the proposed constraint propagation method has the shortest search time, followed by forward checking, arc consistency, and the method without constraint propagation. These results indicate that forward checking limited to the pose constraints as proposed in this paper is the most effective. Meanwhile, the results of Figure 12 demonstrate that forward checking (FC) is more effective than arc consistency (AC) for motion feasibility checking.
Sixth, experiments were conducted to compare the performance of our TAMP framework with existing constraintbased methods, namely, LozanoPérez [18] and Lagriffoul [17]. In LozanoPérez, both the MRV variable ordering heuristics and arc consistency checking with pose constraints are applied. In case of Lagriffoul, both an actiondependent variable ordering heuristics and forward checking with pose constraints are applied. Because their CSP modellings are much different from ours, we reimplement their heuristics and search strategies within our framework. The experimental results in Figure 13 demonstrate that the proposed framework used the shortest search time. LozanoPérez’s method showed the lowest performance because its generalpurpose heuristic, MRV, and arc consistency are not effective for multihop propagation. Lagriffoul’s method is faster than that of LozanoPérez because of its unique variable ordering heuristics and forward checking. However, Lagriffoul’s method only considers the interaction dependency for variable ordering, while our framework considers both inter and intraaction dependencies.
6. Discussion and Conclusion
This paper proposed an efficient constraint satisfaction framework for checking motion feasibility of task plan skeletons as a TAMP solution. The proposed framework not only presents mapping rules that can automatically generate a constraint satisfaction problem (CSP) from the task plan skeleton, but it also provides useful methods to solve the CSP such as preprocessing, unique variable ordering heuristics based on intra and interaction dependencies in a task plan skeleton, and constraint propagation to improve efficiency of motion feasibility checking. Through numerous experiments using the humanoid robot PR2, we confirmed high performance and efficiency of the proposed framework.
However, the current framework has some technical limitations. The proposed framework was applied to relatively simple task environments based on the assumption that the environments are fully observable, static, deterministic, and have a single robot. However, the realworld physical environments do not meet the assumption. They are partially observable, dynamic, stochastic, and have multiple robots or manipulators. Therefore, we plan to extend our framework to deal with dynamics of realworld environments by designing advanced action model and replanning capability. Furthermore, to address uncertainty in state recognition, the current framework will be extended to solve TAMP over belief state space. It will be also interesting to apply the proposed framework to multiagent or multirobot environments.
Appendix
Appendices A–D describe the functions that appear in Section 4.1.2. Table 13 describes the definition of constraints Section 4.1.3.
A. Matrix from Pose
Given a pose p, this function converts p to rotation matrix R. R is obtained as follows:where
B. Matrix from Quaternion
Given a quaternion q, this function converts q to rotation matrix R. R is obtained as follows:where
C. Quaternion from Axis Angle
Given an axis angle, this function converts the axis angle to quaternion q. q is obtained as follows:where
D. Matrix from Axis Angle
Given an axis angle, this function converts the axis angle to rotation matrix R. This function obtains a quaternion from the function of Appendix C and obtains R from the function of Appendix B.
Data Availability
The data used to support the findings of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare that there are no conflicts of interest regarding the publication of this paper.
Acknowledgments
This work was supported by the Technology Innovation Program or Industrial Strategic Technology Development Program (Grant No. 10077538, Development of manipulation technologies in social contexts for humancare service robots) funded by the Ministry of Trade, Industry and Energy (MOTIE, Korea).