Constraint Satisfaction for Motion Feasibility Checking

Task andmotion planning (TAMP) is a key research field for robotic manipulation tasks.+e goal of TAMP is to generate motionfeasible task plan automatically. Existing methods for checking motion feasibility of task plan skeletons have some limitations of semantic-free 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 intraand inter-action 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 themotion feasibility checking in our framework is 1.4x to 6.0x faster than previous ones.


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) , which is a key research field of robotic manipulation tasks. A typical TAMP involves the combination of task planning [38][39][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][42][43] that checks the motion feasibility for determining if the motions can be executed in a physical space. e ultimate goal of TAMP is to combine these to generate a motion-feasible 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. is disadvantage can be supplemented through motion planning. While abstract symbolic knowledge is unknown in motion planning, it can generate collision-free 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 motion-feasible task plan. e left side of Figure 1 shows the task layer including the task planning process. e 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. e interface layer then checks if there is a collision-free path between candidates of neighboring pose parameters with the help of the motion layer. If all of collision-free paths exist, then the candidates are assigned to the pose parameters of the task plan skeleton to generate a motion-feasible task plan. Otherwise, it creates a new task planning problem from motion-related errors (e.g., obstacles), and repeats the same interfacing process. is paper focuses on checking the motion feasibility of the task plan skeleton in the interface layer. Motion feasibility checking involves finding motion-feasible values of unbound pose parameters included in the task plan skeleton. is 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 collision-free paths of multiple actions. To solve this problem effectively, the followings are required: (1) theoretical modelling of the search problem to find motion-feasible 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). e previous study of Lozano-Pé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 on-the-fly CSP. Lagriffoul et al. [17] attempted to model and to solve the motion feasibility checking problem as a general search tree. ese 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 general-purpose 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 collision-free path with high variability. It complicates the search problem and makes planning and execution inefficient in the long run.
is 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 motion-feasible) value of pose parameters. e valid value means that it satisfies constraints such as kinematics and collision-free path. In our framework, the motion feasibility checking is modelled as a constraint satisfaction problem (CSP). e CSP formulation has the advantage that the various types of constraints can be represented equally in a concise form. In addition, effective general-purpose or special-purpose 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. is method reduces the search space substantially. Meanwhile, actions belonging to the task plan skeleton have inter-action dependencies because of logical order. In addition, parameters and preconditions within an action have intra-action dependencies. (2) Based on these inter-and intra-action 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. e 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.

State of Art
We categorize previous works according to their motion feasibility checking methods, as shown in Table 1 Figure 1: An example of combining task and motion planning using the interface layer.
eager method [1][2][3][4][5][6][7][8][9][10][11][12][13][14] alternately performs task planning and motion-feasibility checking step-by-step. On the contrary, the lazy method  postpones its motion-feasibility checking until a complete skeleton of task plan is built. e eager method has the advantage of being able to identify in advance future motion-unfeasible 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. is paper utilizes the lazy method. Especially, Lozano-Pé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, Lozano-Pérez and Kaelbling [18] attempted to model the motion feasibility checking problem as a traditional CSP. In [18], the depth-first 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 on-the-fly 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. e method of [37] is also less scalable because it only considers general-purpose CSP heuristics.
Finally, Lagriffoul et al. [17] attempted to model the motion feasibility checking problem as a general search tree. e nodes of the tree represent pose parameters, and the edges represent collision-free trajectories. Based on the depth-first 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 inter-action 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). e authors of [32] proposed a TAMP framework using a top-k skeleton planner to produce diverse skeletons, guaranteeing that no better solution exists under a current domain description. Moreover, the framework uses a Monte-Carlo Tree Search (MCTS) to solve this stochastic decision-making problem over skeletons and concrete bindings of the action parameters. e 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 multi-agent or multi-robot TAMP problems. e authors of [34] dealt with a new problem of motion planning feasibility checking for task-agent assignment to perform complex tasks using multi-arm mobile manipulators. e authors of [35] presented a multi-robot 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.

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. en, 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 Lazy  Motion planning Task planning f (p) p = <a 1 ,a 2 , a 3 , a 4 > initial state, and sk+1 the goal state, which satisfies the goal condition. Meanwhile, each action a has pose references. e pose references are discrete identifiers for representing the values of the poses still undetermined in the task planning phase. e 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. is task plan skeleton consists of a sequence of actions in which the robot moves object03 to table 02. e 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 task-level 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 Move-BasePickUp must satisfy state conditions such as reach-ableBasePose 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.
is action contains many motions, thus requiring a relatively large number of pose parameters. is action includes the initial hand pose (handPose0), the pregrasping hand pose (handPose1), the grasping hand pose (handPose2), and the post-grasping 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. is 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 <v1, . . ., vk> of pose references in S. e valid values <v1, . . ., vk> 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. e valid values of the undetermined pose parameters must satisfy the state conditions presented in the task-planning 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.
us, 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.

Proposed Solution
e task plan skeleton contains unbound pose references. Values for the pose references must satisfy the motion-related constraints, such as IK and the collision-free path. If any value satisfying all constraints exists, then we can consider that the task plan skeleton is motion feasible. is is a constraint-based 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.  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. e 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.

Formulating Constraint Satisfaction Problem from
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. e types include position of mobile base B, pose of hand H, and position of object O. e 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.

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. is can be represented by the following: where r 0 is a matrix representing an axis angle a � (0, 0, 0), r 1 is a matrix representing a quaternion q � (0, 0, 0, 1), t 0 is a matrix representing a pose p � (1, 0, 0, 0, d, 0, 0), and r 2 is a matrix representing an axis angle a � (0, 0, θ). e 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. is 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. e 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.
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. is can be represented in an equation (3) as follows: where r 0 is a matrix representing a quaternion q � (0, 0, 0, 1), r 1 is a matrix representing an axis angle a � (0, − (π/2), 0), t 0 is a matrix representing a pose p � (1, 0, 0, 0, − d, 0, 0), r 2 is a matrix representing an axis angle a � (0, 0, θ), and t 1 is a matrix representing a pose p � (1, 0, 0, 0, 0, 0, h). is equation is to calculate the position and orientation of the hand relative to the coordinate system of the object.
is 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. 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 p � (1, 0, 0, 0, x, y, z), x, y, and z are constrained as below.
In equation (4), the support plane P is assumed to be rectangular. is 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 . e gray area on the table is the area of the domain. e 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.

Constraints.
e 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. ese 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. ere is an issue of how to deal with the pose of movable object to be affected by the previous action. ere are some ideas for this. In Lozano-Pé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 self-action parameters. For instance, placeableHandPose constraint has only one variable for object pose regardless of the number of place actions. is constraint considers the poses of other movable objects as constraints that are loaded from the environment.
is 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. e path constraints are validBasePath and val-idHandPath, and the remainder is the pose constraint. All constraint descriptions are summarized in a table in the appendix. e classification of constraints according to semantic is used in heuristic design.

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. rough the node consistency, the values that satisfy the unary constraints are left and others are removed. In Table 6, the unary constraints are reach-ableBasePose (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.

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 inter-action dependency and the intra-action dependency.

Variable
Ordering. First, our variable ordering considers the inter-action dependency. We mentioned the movable object issue of our modelling method in section 4.1.3. is variable ordering solves this issue. is takes into account the order between actions. All variables belonging to the earliest ordered actions are visited first. en, 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. en, 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 intra-action dependency between variables orders multiple variables within one action. e intra-action dependency between the variables is as follows.  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 general-purpose CSP heuristic. Table 8 shows the search order of the variables finally determined by this ordering. e variable ordering proposed in this paper is a mixture of generic variable ordering and special-purpose variable ordering.

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. e path constraints are validBasePath and val-idHandPath, and the remainders are the pose constraints.    is constraint classification method can be used to determine the calling order of the constraints. e 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. erefore, if the trajectory is calculated for an invalid goal pose, the search time is greatly wasted. e constraint calling order has the effect of prepruning invalid pose values, thus preventing unnecessary path constraint calls.

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. erefore, 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. erefore, 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. is 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. us, a valid goal pose must be determined first to enable the search for a collisionfree path to the goal pose. e proposed framework solves the constraint satisfaction problem by using the search method shown in Algorithm 1. Algorithm 1 describes a depth-first 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. e 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.
is algorithm propagates pose constraints to variables that are still unbound and neighboring the cur-

Implementation.
Our framework was implemented in Ubuntu 16.04, Python 2.7. e FD (Fast Downward) library [46] was used for task planning, and the TrajOpt library [47] was used for motion planning. e robot task environment was implemented using the OpenRAVE [48] simulator. e modelling and solution methods of the CSP were implemented by extending the python-constraints open-source library [49]. e computing environment was an Intel (R) Core (TM) i7-7700 CPU @ 3.60 GHz, 16 GB memory.

Evaluation.
We performed experiments to evaluate the generality, scalability, efficiency, and optimality of the proposed CSP framework for TAMP. e 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. e 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. e domain set D 2 is twice as large as the domain set D 1 . e sizes of the obstacles were set as shown in Table 11. e 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. e constraints were largely divided into pose and path constraints, and more specifically, constraints on mobile base, hand, and object.
e experimental results show that out of 16      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. e horizontal axis is a problem instance with different values of the experimental parameters, and the vertical axis is the search time.
e experimental results demonstrate that the search with the proposed preprocessing is faster than that without preprocessing. e set of constraints listed in Table 9 includes many unary constraints. erefore, the proposed preprocessing was much effective to reduce the search space.
ird, 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.
e proposed variable ordering method shows the fastest search time, followed by MRV, DH, and random ordering. ese results demonstrate that the proposed region-dependent 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.
e experimental results demonstrate that the search time with constraint ordering is faster than the search time without constraint ordering.
ese results indicate that the proposed constraint ordering to pre-prune 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). e 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. ese 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 constraint-based methods, namely, Lozano-Pérez [18] and Lagriffoul [17]. In Lozano-Pé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. e experimental results in Figure 13 demonstrate that the proposed framework used the shortest search time. Lozano-Pérez's method showed the lowest performance because its general-purpose heuristic, MRV, and arc consistency are not effective for multi-hop propagation. Lagriffoul's method is faster than that of Lozano-Pérez because of its unique variable ordering heuristics and forward checking. However, Lagriffoul's method only considers the inter-action dependency for variable ordering, while our framework considers both inter-and intra-action dependencies.

Discussion and Conclusion
is paper proposed an efficient constraint satisfaction framework for checking motion feasibility of task plan skeletons as a TAMP solution. e 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 pre-processing, unique variable ordering heuristics based on intra-and inter-action dependencies in a task plan skeleton, and constraint propagation to improve efficiency of motion feasibility checking. rough 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. e 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 real-world     When robot is located on base pose B, there must be no collision and the target object must be within radius of robot's arm.

placeableBasePose (B)
When robot is located on base pose B, there must be no collision and the center of table must be within radius of robot's arm.

graspableHandPose (H, B)
When robot's hand is located at hand pose H, robot should be able to grasp the target object by closing fingers. When robot is located on base pose B, there is at least one inverse kinematics for the hand pose H.

preGraspableHandPose (H, H′, B)
When robot's hand is located at hand pose H, the hand must be a little farther than hand pose H′ which is a graspableHandPose for target object. When robot is located on base pose B, there is at least one inverse kinematics for the hand pose H

postGraspableHandPose (H, H′, B)
When robot's hand is located at hand pose H, the hand must be a little higher than hand pose H′ which is a graspableHandPose for target object. When robot is located on base pose B, there is at least one inverse kinematics for the hand pose H.

placeableHandPose (B, O, H)
When robot's hand is located at hand pose H, robot should be able to place the target object on the target location O by opening fingers. When robot is located on base pose B, there is at least one inverse kinematics for the hand pose H.

lowerableHandPose (B, O, H′, H)
When robot's hand is located at hand pose H, the hand must be a little higher than hand pose H′ is a placeableHandPose for target location O. When robot is located on base pose B, there is at least one inverse kinematics for the hand pose H.

retractableHandPose (B, O, H′, H)
When robot's hand is located at hand pose H, the hand must be a little farther than hand pose H′ is a placeableHandPose for target location O. When robot is located on base pose B, there is at least one inverse kinematics for the hand pose H.

placeableObjectPose (O)
When target object is located on target location O, there must be no collision. validBasePath (B, B′) ere must be at least one collision-free path from base pose B to base pose B'. validHandPath (H, H′, B) ere must be at least one collision-free path from hand pose H to hand pose H′ at base pose B.

Problem instances
None Arc 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 multi-agent or multi-robot 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.

B. Matrix from Quaternion
Given a quaternion q, this function converts q to rotation matrix R. R is obtained as follows:

C. Quaternion from Axis Angle
Given an axis angle, this function converts the axis angle to quaternion q. q is obtained as follows:

D. Matrix from Axis Angle
Given an axis angle, this function converts the axis angle to rotation matrix R. is function obtains a quaternion from the function of Appendix C and obtains R from the function of Appendix B.

Data Availability
e data used to support the findings of this study are available from the corresponding author upon request.

Conflicts of Interest
e authors declare that there are no conflicts of interest regarding the publication of this paper.