Online, Interactive User Guidance for High-dimensional, Constrained Motion Planning

1 Introduction

Motion-planning is a fundamental problem in robotics that has been studied for over four decades [Choset et al., 2005; LaValle, 2006; Halperin et al., 2017]. However, efficiently planning paths in high-dimensional, constrained spaces remains an ongoing challenge. One approach to address this challenge is to incorporate user input to guide the motionplanning algorithm. While there has been much work on planning using human demonstration (see, e.g., [Argall et al., 2009; Phillips et al., 2016; Schulman et al., 2016; Ye and Alterovitz, 2017]), there has been far less research on incorporating guidance as an interactive part of the planning loop.

Broadly speaking, interactive planning has been typically used in the context of sampling-based motion-planning algorithms [LaValle, 2006]. User guidance was employed for biasing the sampling scheme of the planner by having the user mark regions in the workspace that should be avoided or explored [Denny et al., 2014; Mehta et al., 2015; Yan et al., 2015; Ranganeni et al., 2017]. Alternatively, interactive devices such as a 3D mouse or a haptic arm have been used to generate paths in a (low-dimensional) configuration space. Such paths were then used by a planner to bias its sampling domain [Blin et al., 2016; Flavigne et al., 2009; Ta¨ıx et al., 2012].

We are interested in planning in high-dimensional, constrained spaces such as those encountered by a humanoid robot (see Fig. 1 and Sec. 2.1). In such settings, workspace regions often give little guidance to the planner due to the high dimension of the configuration space as well as the physical constraints of the robot. Additionally, providing the user guidance in the configuration space is extremely time consuming, even for expert users. Thus, though beneficial, user guidance should be employed scarcely.

Our key insight is that carefully chosen individual configurations suggested by a user can be used to effectively guide the planner when in need. Transforming this insight into a planning framework requires addressing three fundamental questions:

• Q1. When should the planner ask the user for guidance?
• Q2. What form should the user's guidance take?
• Q3. How should the guidance be used by the planner?

Identifying when to obtain guidance (Q1, Sec 4.1) comes in stark contrast to existing approaches-we suggest to only employ user guidance when the planner identifies that it ceases to make significant progress towards the goal. Once obtained (Q2, Sec 4.2), guidance is used to bias the search algorithm towards regions that are likely to be beneficial (Q3, Sec 4.2).

The search algorithm now has the additional task of deducing how much to rely on the user-provided guidance. It is worth emphasizing that obtaining this guidance does not require the user to understand the underlying search algorithm.

We demonstrate our approach using search-based planning algorithms (see, e.g., [Cohen et al., 2014]) that perform a systematic search guided by heuristic functions. Specifically, we use multi-heuristic A* (MHA*) [Aine et al., 2016; Narayanan et al., 2015] which we detail in Sec. 2.2. Our conjecture is that the formulation can, in general, be incorporated with any motion-planning algorithm if it can address the aforementioned questions, however showing its effectiveness on other algorithms is out of the scope of the paper.

After describing our approach at high-level (Sec. 3) we demonstrate how it can be applied to the case of MHA* (Sec. 4). We continue by showing the effectiveness of our planner (Sec. 5). Specifically, we show that this general approach allows to compute highly-constrained motion plans for a spectrum of tasks including climbing stairs, walking under a bar, squeezing through a door, and others, all with the same planner and heuristics. Without our approach, solving such problems requires the design of task-specific planners [Kaneko et al., 2004] and carefully hand-designed domain-dependent heuristics.

2 RelatedWork and Algorithmic Background

2.1 Motion Planning for Humanoid Robots

Humanoid robots, for which we demonstrate our approach, often have several dozens of degrees of freedom making planning a challenging task, especially when taking additional constraints into account such as stability and contact constraints. One approach to plan the motion for such systems, is to use predefined, carefully chosen fixed gaits [Kaneko et al., 2004]. However, when the terrain is uneven, such planners are inadequate at computing stable motions [Hauser et al., 2008]. Another approach is to reduce the size of the search space by decomposing the degrees of freedom into functional groups such as locomotion and manipulation. Then functional-specific algorithms such as footstep planning are applied to the low-dimensional space (see, e.g., [Chestnutt et al., 2005; Kuffner et al., 2001; Perrin et al., 2012; Xia et al., 2009] for a partial list). A high-dimensional planner is then used to "track" the plan generated in the lowdimensional space.

User guidance has been intensively incorporated in controlling the motion of humanoid robots, especially in complex tasks as those presented in the Darpa Robotics Challenge (DRC) [Spenko et al., 2018]. In such settings, guidance ranged from teleoperating the robot to high-level task guidance [McGill et al., 2017; Gray et al., 2017; DeDonato et al., 2017; Marion et al., 2017]. However, as far as we are aware of, in all the aforementioned cases, identification of when to use guidance was done by a human operator and not by the system (except for relatively simple metrics where the system asks for help when it fails to complete some given task). Furthermore, the human guidance was used as a "hard" constraint forcing the system to make use of the guidance. In contrast, in our work the system automatically and independently identifies when it is in need of guidance. This guidance is then seamlessly incorporated as a soft constraint- the system biases the search towards the guidance while continuing to explore the search space as if the guidance was not given.

2.2 Multi Heuristic A* (MHA*)

Multi Heuristic A* (MHA*) [Narayanan et al., 2015; Aine et al., 2016] is a search-based planning algorithm that takes in multiple, possibly inadmissible heuristic functions in addition to a single consistent heuristic termed the anchor heuristic. It then uses the heuristic functions to simultaneously perform a set of weighted-A* [Pohl, 1970]-like searches. Using multiple searches allows the algorithm to efficiently combine the guiding powers of the different heuristic functions.

Specifically, for each search, MHA* uses a separate priority queue associated with each heuristic. The algorithm iterates between the searches in a structured manner that ensures bounds on sub-optimality. This can be done in a round-robin fashion, or using more sophisticated approaches that allow to automatically calibrate the weight given to each heuristic [Phillips et al., 2015].

Part of the efficiency of MHA* is due to the fact that the value of the cost-to-come (the g-value) computed for each state is shared between all the different searches 1 . Sharing cost-to-come values between searches implies that if a better path to a state is discovered by any of the searches, the information is updated in all the priority queues. Moreover, if one search ceases to make progress towards the goal, it can use "promising" states visited by other searches to escape the local minimum.

2.3 Identifying Progress in Search-based Planners

A key component in our work is automatically detecting when our planner requires user guidance. This requires characterizing regions where the planner ceases to make progress and algorithmic tools to detect such regions. These two requirements are closely related to the notion of heuristic depressions [Ishida, 1992] and search vacillation [Dionne et al., 2011; Burns et al., 2013].

A heuristic depression region is a region in the search space where the correlation between the heuristic values and the actual cost-to-go is weak. It is defined as a maximal connected component of states D such that all states in the boundary of D have a heuristic value that is greater than or equal to the heuristic value of any state in D.

Such regions often occur in real-time search algorithms such as LRTA* [Korf, 1990] and LSS-LRTA* [Koenig and Sun, 2009] where the heuristic function is updated as the search progresses. Subsequently, current state-of-the-art algorithms guide the search to avoid states that have been marked as part of a heuristic depression [Hern´andez and Baier, 2012]. As we will see, we will use a slightly different

1 To be precise, there are two variants of MHA* described in [Aine et al., 2016]: Independent and Shared MHA* where the queues do not share and do share the g-values of states, respectively. In this paper when we use the term MHA*, it refers to the latter (shared) variant.

Algorithm 1 User-guided Planning (A)

characterization of when the planner ceases to make progress which we call stagnation regions. For details, see Sec. 4.1.

Search vacillation is a phenonemon when the search explores multiple small paths due to error in the heuristic. We will use the notion of search vacillation to identify when the planner is in a stagnation region (i.e., when it ceases to make progress towards the goal). For details, see Sec. 4.1.

3 Algorithmic Approach-User-guided Planning

To employ our planning framework, we assume that we are given a motion-planning algorithm A that is endowed with two non-standard procedures which are planner dependent. The first, in stagnation region(), identifies when it is in a stagnation region (with or without the guidance), namely when A's search does not progress towards the goal. The second, update user guidance(), incorporates (or removes) the user guidance provided to A.

Equipped with these functions, we can describe our planning framework, detailed in Alg. 1. The framework runs as long as no solution is found (line 1). It runs the planner A (lines 2-3) as long as it continuously makes progress towards the goal (namely, it is not in a stagnation region). Once a stagnation region is identified, user guidance is invoked (line 4) and A is updated to make use of this guidance (line 5). The planner continues to run while using the guidance as long as the planner without the guidance is still in the stagnation region, and the guidance is useful (lines 6-7). Once it escapes the stagnation region (or if the guidance appears to be unhelpful), A is updated to remove the guidance that was provided by the user (line 8).

4 User-guided Planning via MHA*

We demonstrate our general planning framework described in Sec. 3 for the case where the motion-planning algorithm A is MHA*. We assume that MHA* has a set of possibly inadmissible heuristics in addition to the anchor heuristic. We will refer to these heuristics as baseline heuristics. The user guidance will be used to generate dynamic heuristics. In Sec. 4.14.3 we describe how we answer the three questions that were posed in Sec. 1.

4.1 Invoking User Guidance (Q1)

The heuristic functions of search-based planning algorithms, such as MHA*, can be used to estimate in a principled manner when the planner is in a stagnation region (Alg 1, lines 2

and 6). We suggest two alternative methods to identify when the planner ceases to make progress towards the goal, each resulting in a slightly different definition of a stagnation region. The first, which we call vacillation-based detection uses the notion of expansion delays [Dionne et al., 2011]: Let Q be a priority queue and let ecurr be a counter tracking the total number of node expansions performed when using Q. When a node is expanded, for each child node s we store the current expansion number using a counter e(s). Now, the expansion delay for a state s is defined as

A moving average of the expansion delay of the nodes being expanded can be used as a proxy to measure the average progress that a planner makes along any single path. When the heuristic used by a planner is perfect, the expansion delay equals one, while on the other extreme, when performing uniform-cost search, the expansion delay can grow exponentially.

Given some parameter ω > 0 we compute ∆e(ω), the average expansion delay over a moving window of size ω.

Definition 1 Let ω > 0 be some window size and τ be some threshold value. A heuristic h associated with a queue Q is defined to

1. enter a stagnation region if ∆e(ω) ≥ τ,
2. exit a stagnation region if ∆e(ω) = 1

For a visualization of vacillation-based stagnation regions and how they are detected (Def. 1), see Fig. 2.

As we will see in our experimental section, due to the nature of the domain or the heuristic function, vacillationbased detection could be deceptive and not detect regions where the planner is not making significant progress. Thus, we suggest an alternative method, which we call heuristicbased detection. Let Q be a priority queue ordered according to some heuristic function h(·), si be the node expanded from Q at the i′ th iteration and ω1, ω2 and ε be parameters such that ω1 > ω2 and ε is some threshold. We define κQ(i, ω) = mini-ω≤j≤i{h(sj)}. Namely, κQ(i, ω) denotes the minimal value attained by h over the past ω expansions.

Definition 2 A heuristic h associated with a queue Q is defined to be in a (heuristic-based) stagnation region if κQ(i, ω1) ≥ κQ(i - ω2, ω1 - ω2) - ε.

Namely, Q is in a heuristic-based stagnation region if looking at the previous ω1 iterations, there was no reduction (by more than ε) in the minimum value of h in the last ω2 states expanded from Q. For a visualization of heuristic-based stagnation regions and how they are detected (Def. 2), see Fig. 3.

4.2 Form of User Guidance (Q2)

We chose to obtain user guidance (Alg 1, line 4) in the form of an intermediate configuration ˆ q that is used to guide the planner. The framework includes a graphical user interface (GUI) (Fig. 4) capable of depicting the robot and the workspace. Once user guidance is invoked, a configuration in the stagnation region is obtained and the robot is placed in that configuration in the GUI. This allows the user to intuitively try and understand where the planner faces difficulty and how to guide it out of the stagnation region. The user then suggests the guidance ˆ q by moving the robot's joints, end effectors or torso. The tool runs a state validity checker (the same one as used by the planner during the search) in the background which restricts the user from providing invalid configurations, e.g. with respect to collision, joint limits and stability.

We informally experimented with non-expert users, and after some practice with the GUI received positive feedback. Our focus in this work was on the algorithmic aspects of our approach and we leave UI challenges to future work.

4.3 Using User Guidance (Q3)

We assume that MHA* has at least one baseline (possibly inadmissible) heuristic hgoal (in addition to the anchor heuristic) which approximates the cost to reach the goal from every state. Furthermore, we assume that for every configuration q, there exists a heuristic hq where hq(s) estimates the cost to reach q from state s.

Given user guidance in the form of a configuration ˆ q, we dynamically generate a new heuristic

Namely, ˆh estimates the cost to reach the goal via ˆ q (see also [Chakrabarti et al., 1986] for intuition regarding such a heuristic). If the state was reached by passing through ˆ q, then the value of ˆh is simply the estimation of the cost to reach the goal.

Equipped with the heuristic ˆh, we add a new queue to MHA* prioritized using the ˆh. States expanded using this queue will be biased towards ˆ q (see also [Islam et al., 2015] for more details on adding heuristics dynamically to MHA*). Recall that in MHA*, nodes are shared between the different queues. Thus, once a state has been found that can be used to get the planner out of the stagnation region, it will be expanded by the other queues using their heuristics. As the planner escapes the stagnation region, the newly-added queue is removed. In general, we can add a dynamic queue for every baseline heuristic if they are more than one. However, for simplicity, in this work we use a single baseline heuristic and add one dynamically generated queue when the user provides guidance. Since the search runs under the MHA* framework, although the dynamic heuristic ˆ q is inadmissible, we still maintain completeness and bounded sub-optimality guarantees.

For pseudo-code describing each specific function used in Alg. 1 under the framework of MHA*, see Alg. 2. When adding the dynamic heuristic, if the baseline heuristic escaped a stagnation region but the configuration ˆ q was not reached, we suspend the dynamic queue but do not discard it. This is done to first try reusing the last guidance before obtaining a new one. Thus, when the planner will detect that it is in a stagnation region, it will first resume the suspended dynamic heuristic (if one exists). If the baseline heuristic escaped a stagnation region and the configuration ˆ q was reached it will no longer be useful again and hence will be discarded. Finally, if the dynamic heuristic is in a stagnation region then it is discarded and the user will be queried for a new guidance. We emphasize that detecting stagnation is done for each heuristic independently.

For a visualization of the way the algorithm progresses, see Fig. 5. Note that although for the illustrated example, the search happens to pass through the guidance, in general it is not a constraint in the framework.

Avg. guidance time 16.4 ± 2.8

13.6 ± 3.6

6.0 ± 1.5

5.5 ± 0.9

Table 1: Experimental results for the bipedal locomotion and ladder mounting tasks for (a) vacillation-based detection (b) heuristicbased detection averaged over 10 trials.

5 Evaluation

We evaluated the performance of our planning framework on a 34-DOF WAREC humanoid robot [Matsuzawa et al., 2015] which is constrained to maintain static stability at all times. To construct the search space, we used a set of motion primitives which are short kinematically feasible motion sequences. These are used to generate the actions, or the successors, of a state during the search. For additional details see [Dornbush et al., 2018]

For each experiment we used a small set of fairly generic heuristics. This was done to demonstrate that our approach allows to significantly reduce the engineering effort required in carefully crafting domain-specific heuristics. We conducted two experiments which largely differ in the nature of mobility and thus employ different baseline heuristic functions. However, the dynamic heuristic function in both the experiments is the Euclidean distance in the joint 34-DOF space. The parameter values used in both experiments are ω1(200), ω2(50) and ε(50) for the heuristic-based, and τ (30) and ω(10) for the vacillation-based stagnation-region detection. We conducted the same experiments with a wide variety of parameters and obtained only slight differences in the results, demonstrating that our approach is highly robust to the choice of parameters for our domain. In addition, we conducted similar experiments with a 7 DOF arm and with a humanoid climbing stairs while using handrails and obtained similar results to the ones we will shortly show. Those results were omitted due to lack of space.

Planning statistics, averaged over ten different trials, for all experiments are provided in Table 1. In all the experiments, we successfully test the hypothesis that with the help of user guidance, the planner, augmented only with a generic baseline heuristic, can completely solve challenging problems with a small number of guidances. The deviations in the results presented in Table 1 come from the quality of the guidance provided in each run. For the video demonstrating the approach, see 2 .

5.1 Bipedal Locomotion

The first task we considered is bipedal locomotion, depicted in Fig. 1. Here, we employ the adaptive dimensionality framework [Gochev et al., 2011; Gochev et al., 2012; Gochev et al., 2013] which iterates over two stages: an adaptive-planning phase which plans in a low-dimensional space when possible and a tracking phase which plans in the high-dimensional space. Our user-guided planner is integrated within the high-dimensional planner to search for a path that tracks the footstep plan generated in the adaptiveplanning phase.

2 https://youtu.be/r7DHlc6XBC0

The baseline heuristic we used was designed to assist the search with a general walking or stepping capability. For each step the footprint of the target footstep is visualized for the user to be able to better understand where the planner is stuck and how to provide the guidance. In relatively easier scenarios this heuristic would suffice in guiding the search. However if the search encounters harder situations such as those depicted in Fig. 6, then it is likely to get trapped into a stagnation region and would invoke user-guidance.

Results demonstrating the effectiveness of our framework are depicted in Fig. 7. Specifically, we plot the method to detect stagnation regions (average expansion delay and heuristic values for vacillation-based and heuristic-based detection, respectively) as a function of the number of queue expansions. For both methods, we ran our algorithm with and without user guidance. Namely, if a stagnation region was detected, we recorded the state of the planner and then continued to run it once without asking the user for guidance and once using our approach. This was done every time a stagnation region was detected. Results show that without guidance, the planner fails to make any significant progress. On the other hand, when guidance is given, the algorithm escapes the stagnation region and resumes to make progress towards the goal.

Interestingly, vacillation-based detection incurs a lot of noise-notice the large oscillations in the dashed yellow curve in Fig. 7(a). A possible explanation is that even in regions where the planner cannot ultimately progress towards the goal, there may be multiple short feasible paths to explore. In such settings, the expansion delay for each such path will be small while the planner does not really progress towards the goal. Indeed, numerical results stated in Table 1 confirm that our heuristic-based method for stagnation-region detection outperforms the vacillation-based method. We observed similar trends for the second experiment (ladder climbing) where vacillation-based method falsely detect that the stagnation region was exited (plots omitted).

5.2 Mounting onto a Ladder

The second task considered was mounting onto a ladder from a standing position (see Fig. 4) to the desired contact poses. This problem is challenging because the ladder rungs and the robot grasping hooks induce narrow spaces in the configuration space which have to be traversed while maintaining stability, to establish the desired contacts. The baseline heuristic used is the summation of four six-dimensional Euclidean distances between each of the four end effectors and their respective target poses. For numerical results, see Table 1.

6 Discussion

Generic vs. domain-specific heuristic

In Sec. 5 we demonstrated how user guidance allows to solve highly-constrained motion-planning problems in highdimensional spaces with only simple baseline heuristics. An alternative approach would be to add domain-dependent carefully-crafted heuristics [Dornbush et al., 2018; Vijayakumar, 2017] which allows to faster solve the same problems completely autonomously.

When faced with a new problem domian (say, climbing a ladder, crawling on uneven terrain etc.) we can either design additional heuristics to tackle this problem or leverage from user-provided online guidance. If our problem requires planning in multiple, diverse domains this problem is accentuated-should we use a large arsenal of heuristics that can address each domain or should we have a small set of baseline heuristics that will (roughly) address all domains and rely on user guidance when these baseline heuristics fail? There is no clear answer to this question and our approach simply offers a general alternative to existing approaches.

When "bad" guidance is provided

A natural question to ask is what if the user provides a guidance that is not useful? If the planner does not progress due to such guidance it would ask for a new one as it detects stagnation in the dynamic heuristics the same way it does in the baseline heuristic. As can be inferred from the experimental results presented in Sec. 5, for the bipedal locomotion 26% of the guidances were not useful, yet the planner was able to recover after being provided with useful guidance. It is important to note that while using MHA* ensures that a path will be found regardless of the guidance that is provided, unhelpful guidance may drive the search towards unuseful regions and waste computational resources.

Acknowledgements

This research was in part sponsored by ARL, under the Robotics CTA program grant W911NF-10-2-0016, and by NSF Grant IIS-1409549.