Learning the Geometric Meaning of Symbolic Abstractions for Manipulation Planning

Transcription

1 Learning the Geometric Meaning of Symbolic Abstractions for Manipulation Planning Chris Burbridge and Richard Dearden School of Computer Science, University of Birmingham, Edgbaston, Birmingham, B15 2TT, U.K. {C.J.C.Burbridge, Abstract. We present an approach for learning a mapping between geometric states and logical predicates. This mapping is a necessary part of any robotic system that requires task-level reasoning and path planning. Consider a robot tasked with putting a number of cups on a tray. To achieve the goal the robot needs to find positions for all the objects, and if necessary may need to stack one cup inside another to get them all on the tray. This requires translating back and forth between symbolic states that the planner uses such as stacked(cup1,cup2) and geometric states representing the positions and poses of the objects. The mapping we learn in this paper achieves this translation. We learn it from labelled examples, and significantly, learn a representation that can be used in both the forward (from geometric to symbolic) and reverse directions. This enables us to build symbolic representations of scenes the robot observes, and also to translate a desired symbolic state from a plan into a geometric state that the robot can actually achieve through manipulation. We also show how the approach can be used to generate significantly different geometric solutions to support backtracking. We evaluate the work both in simulation and on a robot arm. 1 Introduction The vast majority of robots are currently controlled through hand-written programs. These programs are typically highly specialised to a single task as they include human specified geometric information about the objects and behaviours being performed. A disadvantage of this approach is that building the programs is very time-consuming, often involving a mixture of writing code and recording geometric states of the robot and the objects the robot is interacting with. To make the robot more autonomous, we would like to be able to specify high level goals such as put all the cups on the tray for the robot and use a task level planner to generate a behaviour that achieves the goals. It is clearly desirable to specify goals in this abstract language; the alternative is to specify the exact geometric location of every object, which essentially requires knowing the plan in advance since, for example, if two of the cups need to be stacked to get them all on the tray, this must be specified in the goal. Task level planners [1, 2] work largely in symbolic spaces [3]. World states are expressed using predicates such as on(x,y) and holding(x) and actions are specified in term of the predicates that must be true for the action to be executed the action preconditions and the changes to the predicates that result the action effects. Recent

2 planners (e.g. [2]) may also support numerical or temporal quantities, but for computational reasons they do not support planning in continuous multi-dimensional spaces in the way that path planners (e.g. [4, 5]) do. Robot programs, in contrast, typically involve specifying sequences of geometric positions of the robot and the objects it manipulates, with the results of the manipulations expressed only in the names of functions, or in comments in the code, or not at all. The approach we describe in this paper is to learn a bi-directional mapping between geometric and symbolic states. This means that for any geometric state we can generate an equivalent symbolic state, for example to generate the initial state for the planner. In addition, we can generate a geometric state consistent with a symbolic state. This allows us to generate the sequence of symbolic states that the world would pass through when a task level plan is executed and produce a geometric state for each of them. These geometric states can then be passed to a path planner to generate the actual motion of the robot. Thus the mapping, along with the high-level actions and the planner itself, provide all we need to generate path plans for the robot that achieve task level goals specified by the user. In this work we assume that object recognition is reliable, so we don t need to worry about unrecognised or mis-recognised objects in the world, only about the relationships between them. An alternative to learning a mapping would be to hand-build one. However, this is quite complex in practice due to the dimensionality of the state space required to represent the relationship between two objects (in our representation this comes to 17 dimensions). Furthermore, the learned mapping naturally captures the typical solutions found, so that for example if an object placed on a tray is typically placed close to the centre, then the learned mapping will capture this fact. We use a supervised learning approach to finding the mapping between symbolic and geometric states. We assume that the actual predicates are pre-defined as part of the planning domain, and we leverage the hand-built robot behaviours to generate labelled training data for each predicate. We assume that the labels are either provided by comments or function names in the code for the hand-built programs, or configurations that result from the execution of the programs are hand-labelled to provide training data. We then learn a kernel density estimate [6, 7] for each label (predicate) from the training data. This allows us not only to label unseen geometric states (the forward direction for the mapping), but also, via hill-climbing search, to find geometric states that match a symbolic predicates with high probability (the backwards direction). We extend this to find geometric states for conjunctions of symbolic predicates and also for back-tracking to find multiple, significantly different geometric states for a predicate. As well as evaluating the mapping itself, we also use it in a relatively simple planning system that uses a standard classical planner [2] to generate symbolic plans and our learned mapping to translate them into geometric goals for a path planner (since the emphasis in this paper is on the learned mapping we do not describe the planner in detail). The result is a plan which we show running on a Kuka robot arm. In the next section we examine related work on mapping from symbolic predicates to geometric states and vice versa; in Section 3 we present our approach to learning the mapping; we present experiments to demonstrate the effectiveness of the approach in Section 4, and present our conclusions and future work in Section 5.

3 2 Related Work A number of planning systems have been developed that operate in a mixture of geometric and symbolic states and therefore need to map between them. Probably the best known is asymov [8], which solves tasks very similar to ours using a planner that combines a symbolic planner with a probabilistic roadmap [5] for geometric planning. In common with most of these approaches, they assume they are given a mapping from geometric to symbolic state. In addition, they restrict the objects to certain fixed world positions, so the translation from symbolic to geometric states is trivial. Another recent example is [9], in which the mapping from symbolic to geometric states is handled by geometric suggesters which are hand-built functions for generating geometric states. Our learned mappings should be able to replace the hand-built mappings in either of these approaches. The idea of learning the mapping from geometric to symbolic state has been considered before in the literature. However, what sets our approach apart from others is the idea of using the learned mapping in reverse, generating geometry from symbolic description to enable symbolic planning operators to be utilised to produce geometric effects. Jäkel et al. [10] present an approach to action learning and geometric constraint learning in the context of Programming by Demonstration. After segmenting logs of human demonstrated actions, constraints on the possible motion of the robot arm are learned. However, constraints on object positions are not directly considered. Unlike our approach, they never need to use the mapping in the reverse direction because there is no task-level planning in their system: the task is only for the robot to execute the learned actions in different geometric configurations. The recent work of Sjöö et al. [11, 12] has also addressed spatial relationships. In [11] they consider the geometric meaning of on, and use hand built rules to map from geometric to symbolic state. Although they go on to use the rules to construct a probability function of the on predicate, which gives them similar representational capabilities to the approach we take, the representation chosen does not generalise to other predicates and requires hand construction. In [12], the same authors address the issue of hand building rules, presenting a system than learns to classify object relationships by generating samples in simulation. While classification of several relations is successful, they are only interested in learning the mapping from geometric to symbolic states; using the mapping in reverse is not considered. Furthermore, the geometric state is described by a 93 dimensional feature vector comprising of contact points, surface normals and object poses, and the symbolic object relationships are learned from this feature vector. The high dimensionality means that far more training data is required to learn a good mapping and makes adapting their approach to compute the reverse mapping impractical. An approach similar to that in [12] is presented in [13]. Again, contact points are used, this time between segmented point clouds. However, the use of K-Nearest neighbours for classification again prevents using the classifier in reverse.

4 3 Symbolic to Geometric Mapping Our approach is to learn a model of the probability density function p(c G) where C is a symbolic state clause, G is geometric state, and a probability of 1.0 indicates that C holds in the given G. Given an entire symbolic state S = C 1 C 2... C n, assuming that the probability of each clause is independent, the probability of it holding in a given geometric state can then be calculated as p(s G) = n p(c i G). (1) i=1 Our aim is to learn a classifier capable of producing class probability estimates from a number of labelled positive and negative examples of (G, C) pairs as discussed in the introduction. Most importantly however, we choose both a p.d.f model and a geometric state representation that allow optimising the probability and turning a given symbolic state representation back into an actual geometric state. 3.1 Geometric state representation For a symbolic predicate of two objects A and B, we define the geometric state representation as: G = [ S A S B TB A R ] A R B (2) where S o, R o are the size and rotation of object o respectively, and TB A is the relative translation between objects A and B. Using the difference in position between the objects rather than absolute positions provides better generalisation from the examples. However, the same can not be done with the rotation as many predicates require specific orientations (pouring for example). We represent the rotations using quaternions, taking the geometric state vector to 17 dimensions. The size of the each object is expressed as the dimensions of its bounding box. Including this in the state allows us to learn just a single general distribution per predicate rather than, for example, needing to learn distributions for both above(cup,tray) and above(cup,cup). 3.2 Probability model It is important to have a smooth continuous representation of p(c G). This allows points of high probability in the multi-dimensional space to be located using numerical methods, which in turn allows finding geometric representations for a given symbolic state. Modelling the probability using Gaussian Kernel Density Estimation (KDE) provides such a smooth function. Given a set of p example geometric states G P that a predicate holds in, and a set of n geometric states G N in which the same predicate does not hold, we model the probability as: p(c G) = P C (G) P C (G) + N C (G), (3)

5 P(G) / (P(G)+N(G)) Positive examples Probability function Negative examples Positive examples G Fig. 1: 1D example of a probability distribution modelled using equation 3. where P C (G) and N C (G) are the multivariate KDEs of the positive and negative datasets for C respectively, evaluated at G: P C (x) = 1 p e GP i x 2 /h 2 (4) p i=1 N C (x) = 1 n e GN i x 2 /h 2, (5) n i=1 where h is the kernel bandwidth. We use the FigTree Fast Gaussian Transform library [7] for efficient KDE implementation. Figure 1 shows an example of the probability model for a one dimensional case. It is clear that in areas of negative examples, the function yields near zero and in areas of positive examples the function yields values of near 1. Importantly, in the boundary between positive and negative examples the function varies smoothly with a smoothness dependent upon the width of the Gaussian example points either side. This allows numerical hill-climbing techniques as described in the following section. 3.3 Using the learned mapping The learned probability distribution can now be used both to classify given geometric states into symbolic states, and to generate geometric states that comply with desired symbolic states. Classification can be achieved by thresholding the class probability at some sensible value, depending upon how certain the classification needs to be. The more interesting case of using the learned distribution to generate geometric states matching desired symbolic configurations is more demanding. For a single predicate goal we achieve this by maximising the 17D probability function, keeping the 6 elements corresponding to object size constant. The smooth and continuous nature of the function allows maximisation to be carried out numerically using hill climbing from an initial estimate selected from G P. We found the Broyden-Fletcher-Goldfarb-Shanno optimisation method to perform suitably well for this purpose.

6 When considering more than one predicate clause using (1), the dimensionality of the space increases and dependencies between objects have to be taken into account. When an object appears in multiple clauses, the size and orientation of the object only appear once in the geometric state. In addition to this, the translations between objects are never allowed to form a cycle. For example, if the state contains the translation between A and B, and between B and C, then there must be no translation between A and C. We avoid these problems by optimising over the direct pose of all non-fixed objects in one shot, evaluating the poses using the relative translations between objects as described in Section 3.1. As an example consider using the approach to find a geometric state for the 5 clause symbolic state (and (above cup1 tray) (touching cup1 tray) (above cup2 tray) (touching cup2 tray) (not (touching cup1 cup2))). If each predicate was treated independently then the search space would contain 85 dimensions. Removing the repeated dimensions for the orientations of objects appearing in the state multiple times, and setting the sizes of all the objects that the state concerns constant leaves a search space of 27D. However, optimising directly in this space could lead to an impossible geometric state as the space contains a loop of transformations: if a transformation between cup1 and the tray is found, and a transformation between cup2 and the tray is found, then the transformation between cup1 and cup2 has to be fixed and can not be found independently. To avoid inconsistencies we optimise in the space of absolute object poses not relative poses. In this example, the optimisation vector comprises of a quaternion and a position for each object considered in the state (3 objects, 21D). When calculating the probability of a state in this form, each relative translation between object is calculated on the fly. 3.4 Generating multiple candidate geometric states Our approach for generating geometric states for given symbolic states outlined in the previous section will provide only a single geometric state with a high probability of complying with the requested symbolic description. However, the reverse mapping is of a one-to-many nature, and in some cases multiple candidate geometric states might be required. A key example of this is when performing hybrid planning, in which the robot may need to decide where to place a cup. If the first chosen place is not possible, because it is blocked by an obstacle for example, alternative candidate locations that are significantly different from the first may need to be selected and tested during a back-tracking stage. The nature of our selected model lends itself well to a straight forward solution to this: for each generated geometric state, we mask the probability distribution (Equation 1) temporarily decreasing the probability of geometric states already created thereby forcing the maximisation to select an alternative point. For the mask we again select a Kernel Density estimate for smoothness: [ Q n+1 = arg max p(s G) G n e t(qi) t(g) 2 /h 2] (6) i=1

7 (a) (b) (c) (d) Fig. 2: Probability distributions superimposed on a geometric state. (a) Tray and two cups. (b) Probability of a cup above the table. (c) Probability of a cup above the table and not touching other cups. (d) Probability of a cup above and touching other cup. where Q n is the n th generated geometric state corresponding to the desired symbolic state S and t(g) is a function providing a vector of positions of objects in state G. Initially no masks are present and n = 0. 4 Experiments and Results To test our system, example geometric states of two cups and a tray were created as discussed in the introduction. Each state was labelled to indicate whether one cup was above and/or touching the tray or the other cup. The data set for the above predicate comprised of 328 positive and 1672 negative examples, and the data set for the touching predicate comprised of 384 positive and 1872 negative examples. These samples were then used to create the 17 dimensional probability distribution detailed in Section 3.2. Examples of the x and y dimensions of the distribution with z 3cm above the tray are shown in Figure 2. Each point on the plane is coloured according to the probability that a cup placed at that (X,Y,Z) position meets the specific symbolic state: sub image (b) shows the probability that the position is above the tray; (c) above the tray and not touching either other cup; (c) above and touching the cup on the right. The bandwidth parameter (variance of the Gaussians) to the Kernel density estimation was selected manually. Larger values make the boundary transitions slower, while low values lead to abrupt transitions that are more difficult for the optimisation. 4.1 The forward mapping To evaluate the classifier in the forward direction we carried out leave-one-out crossvalidation for each predicate. Carrying out the cross-validation for several classification thresholds suggested an optimum value of about 0.8. We found that with significantly less training samples than the already small number that we demonstrate with here, the classification results in large numbers of false negatives. The resulting confusion matrices when using a threshold of 0.8 were as follows: Predicted above not above Actual above not above Actual Predicted touching not touching touching not touching

8 Table 1: Classification probabilities for the states shown in Figure 3 State (a) (b) (c) (d) (above cup tray) (touching cup tray) (above cup2 tray) (touching cup2 tray) (touching cup cup2) (above cup cup2) (a) (b) (c) (d) Fig. 3: The geometric states tested in Table 1 Figure 3 illustrates the forward mapping by showing four simulated geometric configurations. For each of them, we compute the probability of a number of symbolic predicates, with the results in Table 1. From the table it can be seen that even with a higher threshold in the region of 0.9 these states are classified correctly. Although the geometric configurations differ from the examples that the system was trained with, the choice of representation for the probability distribution means that states falling between two positive examples are awarded near 1 probability. The smooth nature of the probability estimate can be further seen in example (d) (one cup stacked inside another) when deciding if the upper cup is touching the tray or not. A probability of indicates that it is close to the tray, but not as close as the cup below which is touching with probability greater than The reverse mapping Figure 4 shows four geometric configurations automatically generated to match the compound states listed in Table 2. States (b) and (c) demonstrate the result of combining the individual predicate distributions. Trying to find a geometric state that has multiple cups on the tray, but not touching each other results in a process somewhat like a spring-and-mass system during hill climbing where the cups are pushing each other apart while the tray is pulling them towards it. The end configuration is the cups on the tray and as far apart from each other as possible. To demonstrate generating multiple different geometric solutions, Figure 5 shows a two dimensional slice through the masked PDFs on a single X,Y plane 3 cm above the tray resulting from generating 6 positions for a cup on a tray using the masking approach detailed in Section 3.4. The effect of the Gaussian shape of the masks where the cups are placed can be seen in the distribution; a wider kernel width will mask

9 Table 2: Classification probabilities for the states shown in Figure 4 Goal state supplied Probability (a) (and (above cup tray) (touching cup tray)) (and (above cup tray) (touching cup tray) (above cup2 tray) (touching (b) cup2 tray) (not (touching cup cup2))) (and (above cup tray) (touching cup tray) (above cup2 tray) (touching (c) cup2 tray) (above cup3 tray) (touching cup3 tray) (not (touching cup cup2)) (not (touching cup cup3)) (not (touching cup2 cup3))) (and (above cup tray) (touching cup tray) (above cup2 tray) (touching (d) cup2 tray) (above cup3 cup2) (touching cup3 cup2) (not (touching cup cup2))) (a) (b) (c) (d) Fig. 4: Geometric states generated for the symbolic states give in Table 2. more of the distribution leading to the generated positions being further apart, while a narrower kernel width would have the opposite effect. 4.3 Robotic Manipulation Planning To demonstrate the applicability and usefulness of our approach in a real world robotics context we used the learned predicates in a symbolic planning system to generate plans for execution by a robot manipulator. We constructed a planning domain description in PDDL [3] comprising actions to pick objects and place them on other objects. For each symbolic action we created a chunk of robot code to perform the action, with additional parameters for the geometric positions. The actions make calls to an RRT path-planner to move the 6 DOF arm between poses. We tested the approach using the Fast Downward[2] symbolic planner with the goal of three cups being above and touching the tray and the fourth above and touching another. The planner generated the sequence of actions necessary to arrange the cups, but only provides the symbolic effect of actions. In order to turn the symbolic plan into a full geometric plan ready for executing on the robot, we then simulated the plan symbolically to generate the sequence of states the robot will visit when executing the plan. Each of these states consists of a set of predicates that is true at that point in the plan. For each state, we then used the geometric to symbolic mapping in reverse to generate a geometric state in which the symbolic state predicates would be true with high probability. We do this state-by-state translation in reverse order and when we search for a corresponding geometric state we only allow objects that have been touched by the robot up to that point in the plan to be moved.

10 Cup position masked Tray Mask after 1 st cup Mask after 2 nd cup Mask after 3 rd cup Mask after 4 th cup Mask after 5 th cup Mask after 6 th cup Fig. 5: Generating multiple candidate geometries for the symbolic state (and (above cup tray) (touching cup tray)) by masking the probability distribution after each candidate is generated. After generating the geometric information for each action in the plan it was directly executed on the real robot as shown in Fig The robot used was an industrial arm (Kuka KR5-Sixx R850) with a two finger gripper (RoadNarrows Graboid). Cup localisation was performed using a Kinect RGBD camera. The point cloud was segmented into clusters above the table plane, with each cluster centroid giving the location of an object. The transformation from Kinect frame to robot frame was calculated using a calibration board at a known location in the robot frame. 5 Discussion and Future Work As we argued above, the mapping between geometric and symbolic states is a necessary part of any robotic system that needs high-level reasoning to achieve tasks. We have shown that such a mapping can be learned from examples and that the mapping can successfully be used in both directions. In addition, we have demonstrated the usefulness of the mapping in a real planning task with a simple planner. At present we have only learned two predicates (the most useful ones for the task), but the approach generalises to more predicates, and as we show in the experiments, the reverse mapping can cope with conjunctions of predicates, although performance does depend on the number of predicates in the conjunction. At present, the parameters of the learned Gaussians are set by hand. There has been significant work in the learning literature on learning the parameters of the models, and 1 Video of execution available at:

11 (a) (b) (c) (d) Fig. 6: The test planning scenario on the Kuka arm. (a) The initial configuration. (b) Ready to grasp a cup. (c) Cup above table. (d) Cup above another cup. we plan to incorporate this work in future. This is particularly important because at present we assume the Gaussians have the same variance in every direction, which is clearly not true in the case, for example, of the touching predicate, which has a very tight variance in the vertical direction. One interesting observation from the experimental task was that there is a third predicate needed to perform well in cup stacking. This new predicate captures the idea that it is safe to release a held cup to form a stack. At present, our planner assumes that above and touching are sufficient, but in fact the held cup should be inside the other to minimise the distance it falls when released. While we could simply add this predicate to the planning domain and learn it as before, one area we are investigating is whether we can use an unsupervised approach to recognise from the examples that there is a consistent relationship between the objects whenever a cup is stacked in another. Similarly, unsupervised learning from arm trajectories might yield constraints such as that cups should be kept open-end-up when moving them.

12 Finally, the experiments shown here perform planning hierarchically: first the symbolic plan is found, then the mapping is used to generate corresponding geometric goals, and then a path planner is used. As the task gets more complex, this approach will fail as the path planner will fail to solve the geometric problem. In these cases, what is needed is a fully hybrid planner as in, for example [8]. We are currently working on integrating this approach with a hybrid planner, and also with execution monitoring and repair to make it more robust to unpredictability in the world. Acknowledgements This work was supported by the European Community s Seventh Framework Programme as part of the GeRT project, grant agreement number ICT References 1. Blum, A., Furst, M.: Fast planning through planning graph analysis. Artificial Intelligence (90) (1997) Helmert, M.: The Fast Downward planning system. Journal of Artificial Intelligence Research 26 (2006) McDermott, D., Ghallab, M., Howe, A., Knoblock, C.A., Ram, A., Veloso, M., Weld, D.S., Wilkins, D.E.: PDDL The planning domain definition language. Technical Report DCS TR-1165, Yale University, New Haven, Connecticut (1998) 4. James J. Kuffner, J., LaValle, S.M.: RRT-connect: An efficient approach to single-query path planning. Proceedings of the IEEE International Conference on Robotics and Automation (ICRA-00) (2000) Siméon, T., Laumond, J., Nissoux, C.: Visibility based probabilistic roadmaps for motion planning. Advanced Robotics Journal (2000) 6. Scott, D.W.: Multivariate Density Estimation: Theory, Practice, and Visualization. John Wiley and Sons, Inc., Hoboken, NJ, USA (2008) 7. Morariu, V.I., Srinivasan, B.V., Raykar, V.C., Duraiswami, R., Davis, L.S.: Automatic online tuning for fast Gaussian summation. In: Advances in Neural Information Processing Systems (NIPS). (2008) 8. Gravot, F., Cambon, S., Alami, R.: asymov: A planner that deals with intricate symbolic and geometric problems. In: ISRR. Volume 15 of Springer Tracts in Advanced Robotics., Springer (2003) Kaelbling, L.P., Lozano-Pérez, T.: Hierarchical task and motion planning in the now. In: Proceedings of Robotics and Automation (ICRA). (2011) Jäkel, R., Schmidt-Rohr, S., Lösch, M., Kasper, A., Dillmann, R.: Learning of generalized manipulation strategies in the context of programming by demonstration. In: Humanoid Robots (Humanoids), th IEEE-RAS International Conference on, IEEE (2010) Sjöö, K., Aydemir, A., Morwald, T., Zhou, K., Jensfelt, P.: Mechanical support as a spatial abstraction for mobile robots. In: Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference on, IEEE (2010) Sjöö, K., Jensfelt, P.: Learning spatial relations from functional simulation. In: Intelligent Robots and Systems (IROS), 2011 IEEE/RSJ International Conference on, IEEE (2011) Rosman, B., Ramamoorthy, S.: Learning spatial relationships between objects. The International Journal of Robotics Research 30(11) (2011)