Harmonious Sampling for Mobile Manipulation Planning

Harmonious Sampling for Mobile Manipulation Planning

Mincheul Kang, Donghyuk Kim and Sung-Eui Yoon Mincheul Kang (mincheul.kang@kaist.ac.kr) and Donghyuk Kim (donghyuk.kim@kaist.ac.kr) are with the School of Computing, and Sung-Eui Yoon (Corresponding author, sungeui@kaist.edu) is with the Faculty of School of Computing, KAIST at Daejeon, Korea 34141

Mobile manipulation planning commonly adopts a decoupled approach that performs planning separately on the base and the manipulator. While this approach is fast, it can generate sub-optimal paths. Another direction is a coupled approach jointly adjusting the base and manipulator in a high-dimensional configuration space. This coupled approach addresses sub-optimality and incompleteness of the decoupled approach, but has not been widely used due to its excessive computational overhead. Given this trade-off space, we present a simple, yet effective mobile manipulation sampling method, harmonious sampling, to perform the coupled approach mainly in difficult regions, where we need to simultaneously maneuver the base and the manipulator. Our method identifies such difficult regions through a low-dimensional base space by utilizing a reachability map given the target end-effector pose and narrow passage detected by generalized Voronoi diagram. For the rest of simple regions, we perform sampling mainly on the base configurations with a predefined joint configuration, accelerating the planning process. We compare our method with the decoupled and coupled approaches in six different problems with varying difficulty. Our method shows meaningful improvements experimentally in terms of time to find an initial solution (up to 5.6 times faster) and final solution cost (up to 17 lower) over the decoupled approach, especially in difficult scenes with narrow space. We also demonstrate these benefits with a real, mobile Hubo robot.

I Introduction

Mobile manipulation planning has been attracted much attention in recent years since the rise of demands for autonomous indoor services, e.g., households tasks or delivery services. Mobile manipulators usually equipped with wheels on a base for the mobility perform various manipulation tasks, e.g., a pick-and-place problem, with robotic arms.

For the mobile manipulation planning problem, a target end-effector pose is given. Given the pose, we first compute its base configuration by utilizing a reachability inversion [26] and then set joints of the manipulator based on inverse kinematics for computing a goal configuration in the joint configuration space (C-space) consisting of the base and manipulator. Unfortunately, this joint C-space can be high-dimensional, and thus tends to lead a slow planning process.

Therefore, a decoupled approach that plans separately for the base and the manipulator has been commonly used in practice, since this decoupled approach works relatively fast thanks to planning in separated and thus lower-dimensional spaces [5]. Nonetheless, this decoupled approach may find sub-optimal results or let the manipulator to place in a narrow space due to the fixed base used for manipulation planning. This is mainly caused by its decoupled planning and sampling that does not consider the joint planning between the base and manipulator.

Fig. 1: This figure shows a sequence of the mobile Hubo robot grasping the yellow beverage. The heat map on the bottom right represents a sample density in terms of the 2D floor projected from samples generated from our harmonious sampling in the joint C-space consisting of the base and the manipulator. Samples are distributed intensively near the goal configuration, resulting in efficient and effective exploration of solution paths.

Main contributions. In this work, we present a simple, yet effective mobile manipulation sampling method, harmonious sampling, for optimal mobile manipulation planning, to efficiently explore a high-dimensional C-space consisting of both base and manipulator (Fig. 1). Our harmonious sampling adjusts the sampling space for the base and the manipulator, and guides to sample more on difficult regions where we need to simultaneously change the configurations of both base and manipulator (Sec. IV-B). We identify such regions based on a low-dimensional base space, especially, manipulation regions that are computed by considering the target end-effector pose and obstacles creating narrow passages (Sec. IV-A). For the rest of regions that are guided from the base regions, we sample configurations of the base body, while the manipulator has a predefined configuration. We also suggest a region-specific k-nearest neighbor search for effectively connecting samples generated differently by these two different regions (Sec. IV-C).

To validate our method, we compare our method with the coupled and decoupled approaches for the base and manipulator in six different problems with different characteristics (Sec. V-A). Overall, we are able to observe that our method works robustly across different scenes, while the other tested methods work well in some cases, but not in other cases (Sec. V-B). Furthermore, we test our approach with the mobile Hubo in a real environment mimicking a convenient store and observe similar results (Sec. V-C).

Ii Related Work

In this section, we discuss prior work on planning for mobile manipulators.

Ii-a Sampling-based planning

The sampling-based approach [16, 19] is one of the most prominent strategies, which has successfully tackled down the high-dimensional problems thanks to its scalable space representation with the probabilistic completeness. In addition, to get efficient movement, many manipulation approaches have used to optimal motion planning algorithms such as Informed RRT [7], BIT [8], FMT [14], and LazyPRM [12], since Karaman et al. [15] introduced RRT and PRM. Burget et al. [3] suggested BIRRT that extends the Informed RRT towards a bidirectional search for a task-constrained mobile manipulator. Schmitt et al. [23] proposed an optimal manipulation planner for continuous grasps, placements, and actions by connecting the configurations. For achieving the almost-sure asymptotic optimality of mobile manipulation planning, our method is based on LazyPRM, while its sampling is guided by our proposed harmonious sampling.

Guided or biased sampling with various heuristic has been widely used and improved the performance of the optimal planners [4, 1, 22, 17]. While these methods guided sampling for a given joint C-space, our harmonious sampling adaptively adjusts the sampling dimensionality for the base and manipulator configurations in mobile manipulation. Interestingly, Gochev et al. [10, 11] suggested a dimensionality-reduction algorithm to efficiently handle high-dimensional problems with adaptive dimensionality, which is, however, applicable to only grid-based search algorithms with resolution-completeness. On the other hand, our method enables the adaptive dimensionality for sampling to the sampling-based approach for designing more effective exploration to the mobile manipulation problem.

Ii-B Mobile manipulation planning

In order to plan an optimal trajectory for a mobile manipulation planning problem, we have to consider the degrees-of-freedom (DoFs) of both robotic manipulator and its base body. Unfortunately, constructing a random geometric graph in such high-dimensional C-spaces with conventional sampling-based planners can be a significant burden due to the curse of dimensionality.

To alleviate the complexity, a divide and conquer strategy has been widely used [5, 24, 9]. By partitioning the entire planning problem into a set of sub-problems (e.g., goal configuration generation and path planning separately for base body and manipulator), it can efficiently subdivide the search space into a set of sub-problem, while achieving a reasonable quality of the robot trajectory.

For goal configuration generation, there have been several works using reachability inversion [26, 6, 13]. These approaches compute a set of suitable base positions reaching the target end-effector pose in consideration of reachability for the manipulator. The goal configurations can be then computed by inverse kinematics (IK) with the constructed reachability map to identify better robot placements.

Given a target end-effector pose in the workspace, various goal configurations can be found during the planning process by adding and considering multiple goal configurations [25, 2]. We adopt this approach of considering multiple goal configurations to find better ones among possible goal configurations. Since our harmonious sampling effectively explores the joint C-space, we can naturally consider these multiple goal configurations for the base and the manipulator in the planning process.

Iii Overview

In this section, we give an overview of our approach. We present our adaptive sampling algorithm to effectively explore the whole configurations of a given mobile manipulation problem.

The main task of mobile manipulation is to reach a given target end-effector pose, , in the task space. Due to the high dimensionality of the mobile manipulation problem, separate planning, i.e., decoupled planning, of the base body and manipulator is commonly adopted. Unfortunately, the manipulator not only has high DoFs, but also is easily placed in a narrow space, because the configurations computed from the given target pose are typically located very close to obstacles such as shelves and objects. Manipulation in this narrow space with the decoupled planning may lead to sub-optimality and increase the overall planning time due to the fixed base chosen from the separately-run base planning for manipulation planning. To alleviate these problems, we explore the whole C-space effectively through our harmonious sampler guided by a low-dimensional base space.

In this paper, we assume an omnidirectional mobile manipulator robot, whose base configuration can be represented by three parameters: 2D position and its orientation (). We define its space to be a base space, , and partition the 3D base space into a set of manipulation regions and the rest . We represent these regions in a grid form for efficient processing. Intuitively, corresponds to difficult regions such that simultaneously adjusting the base, , and manipulator, , is required. As a result, in these manipulation regions, high-dimensional planning in the joint C-space is necessary to get a feasible solution. On the other hand, in the base regions , mainly considering the DoFs of the base body is performed to reduce the dimensionality in less cluttered regions. Note that our sampling process is guided by the 3D base space , but all of generated samples reside in the joint C-space consisting of DoFs of both the base and manipulator; the samples generated with the base regions have a predefined configuration for joints.

Input: : target end-effector pose, : predefined configuration of joints. : initial configuration
1     while  do
2       if  or  then
3               if  then
4                   Insert to and
5            else
6                   continue
8      else
9               if  then
10                   Insert to
11            else
12                   continue
14        foreach  do
15             Insert to
16      UpdateSolutionPath()
Algorithm 1 The proposed algorithm

Alg. 1 shows our overall approach based on LazyPRM [12]. We briefly explain how LazyPRM constructs a graph consisting of a vertex set, , and a edge set, , with our proposed methods highlighted in the yellow color. We first identify using the reachability map and generalized Voronoi graph (GVG), and then construct a sampling distributor to harmonize sampling of two identified regions, and (line 2-3 of Alg. 1). We design such a sampler, a harmonious sampler , to consider the ratio of hyper-volumes of these two different sampling spaces to draw more samples on a larger space (line 12 of Alg. 1).

By drawing more samples in a larger space, there exists a connection issue between samples generated differently from those two different regions. To ameliorate this issue, we propose a region-specific k-nearest neighbor (k-NN) search that effectively links between samples of different regions (line 17 of Alg. 1). In addition, to select the best one among various goal configurations, , from a target end-effector pose , we dynamically add goal configurations in the planning process (line 6 of Alg. 1). Lastly, we compute the shortest path from to on the constructed graph , and then check collisions for edges of the path (line 20 of Alg. 1). This is because LazyPRM delays edge collision checking after finding a better path to reduce the overhead of collision checking (line 18-19 of Alg. 1).

Iv Main approach

(a) Reachability map.
(b) A narrow passage.
Fig. 2: These figures show our manipulation regions (visualized in 2D) computed by the target end-effector pose and the narrow passage. The blue cells indicate reachability maps computed from the target end-effector pose (blue box). The green and purple spheres represent the geometric volumes of the base and the manipulator , respectively. The red cells on the 2D floor visualize manipulation regions caused by obstacles such that has no collision, but has collision. Black lines on the floor show the edges of the GVG.

In this section, we explain how to identify manipulation regions and how to sample them harmoniously across different regions. We also introduce a region-specific k-nearest neighbor (k-NN) search for effective planning with our harmonious sampling.

Iv-a Manipulation Region Identification

We compute the manipulation regions in the base space, to efficiently identify difficult regions in the joint C-space where simultaneously adjusting the base and the manipulator. To identify such regions, we adopt the reachability map [26] from the target end-effector pose , and utilize the GVG to deal with narrow passage problems caused by various objects. Note that while we compute the manipulation regions in the base space, their sampling is done in the joint C-space consisting of the base and manipulator.

Reaching the target end-effector pose. The manipulator is required to move to reach the given target end-effector pose . For realizing the task, we first aim to locate our robot in an appropriate base location in the base space . For computing those base locations, we use a reachability map from the end-effector pose.

The reachability map defined in the 3D base space represents a set of suitable base configurations computed for realizing the target end-effector pose . The blue cells in Fig. 2(a) are 2D example cells of the reachability map computed from the target end-effector pose shown in the figure. When the base is located in the reachability map, it is guaranteed to reach the target end-effector pose [26]. We set our 3D manipulation regions to cover those reachability map. Once a random sample for the base configuration is generated in the manipulation region, we also sample a random configuration for the manipulator, to explore the whole high-dimensional space including the base and manipulator configurations. Details on sample generation is in Sec. IV-B.

Avoiding obstacles in narrow passage. When we have many obstacles, it can potentially create narrow passages. In these narrow passages, it is required to simultaneously adjust the base and manipulator to identify a collision-free path. As a result, we expand the manipulation regions to include such narrow passages as well.

To effectively identify such narrow passages, we construct GVG for the workspace analysis. Each vertex and edge of GVG is associated with its closest obstacles, and thus accessing those edges provides useful information on identifying narrow passages [18]. We also adopt a sphere expansion algorithm [27], by simplifying the geometric description of a robot as a set of spheres. Specifically, we first represent the geometric volume of the base and manipulator by spheres, which are represented by and , respectively. We traverse edges of GVG in 2D workspace (, ) and perform collision detection between our sphere representations with a random orientation and the environment. We then identify regions where has no collision, but has collision to be difficult regions for manipulation, and thus add them to our 3D manipulation regions . Fig. 2(b) shows an example of GVG in the tested environment and our sphere representations of the base and the manipulator of the predefined pose.

Input: : harmonious sampler, : predefined configuration of joints.
1 if  then
2       if  then
4      else
Algorithm 2 Harmonious sampling

Iv-B Harmonious Sampling

The purpose of our harmonious sampler is to balance the sampling distribution between two different regions, i.e., and . To achieve our goal, we design to adjust the sampling probability adaptively by considering the ratio of hyper-volumes of these two different sampling spaces to draw more samples on a larger space. Note that the manipulation regions is defined in the base space , but is associated with the larger sampling space consisting of the base and manipulator, i.e., the joint high-dimensional C-space.

Our harmonious sampler first generates a random sample for the base configuration (line 2 of Alg. 2) and then we check whether the sample is in either or . If , we also sample a random configuration of the manipulator, (line 3-4 of Alg. 2). Otherwise, is initialized with predefined values of joints, , which can be initial values of joints (line 5-6 of Alg. 2). Note that is composed of and (=).

To generate a random sample for the base configuration according to our harmonious sampler , we use a probability mass function (PMF) defined in a grid form over the 3D base space. Each cell of the grid over the 3D base space has only two types of indicating whether the cell is in or . Our harmonious sampler then uses the inverse transform sampling method that works for an arbitrary sampling PMF.

To compute a sampling probability for two regions and , we utilize the notion of the sampling hyper-volume associated to each cell, . Specifically, when a cell is on the manipulation region , its sampling probability is computed based on the hyper-volume of its sampling space, , which is simply calculated by multiplying the intervals between the lower bound, , and upper bound, , for each parameter space of its sampling space:


where is the DoF associated with the sampling space of , and is the weight to match the units for each dimension (Sec. IV-D). This approach is also used for computing a sampling probability for cells on the base regions .

The PMF for our harmonious sampler of cells in the base space can be defined as:


where is the number of cells in the base space , whose cell index is . Consequently, the harmonious sampler varies the sampling probability according to sampling DoFs associated with each cell, i.e. three sampling DoFs for and ten sampling DoFs for .

Note that our harmonious sampler is simple yet useful, but cannot guarantee the optimality. To ensure the probabilistic completeness and the asymptotic optimality of our harmonious sampling, we use both our harmonious sampler at a probability of and the uniform sampler over the entire sampling space at a probability of  [1, 22] (Alg. 2).

Iv-C Linking Different Regions

Our harmonious sampling adaptively adjusts its sampling space depending on whether a sample is generated on the base regions or manipulation regions . Note that we generate more samples on the joint C-space associated with , compared to that associated with , thanks to the wider sampling DoF associated with . Fig. 3 illustrates the different sampling densities on the joint C-space; in this example, and are illustrated in the bottom one-dimensional space.

Fig. 3: Our region-specific k-NN links samples generated through different regions. Blue and orange dotted lines, i.e., connections, are made by our approach for the red dot.

We found that this varying sampling probability causes a sub-optimal performance, especially due to sub-optimal linking along the boundary of two different regions. For example, when we naively perform k-NN search for samples (e.g., the red dot in Fig. 3) generated from the manipulation regions , identified samples are likely to be ones from due to its high sample probability causing small distances between samples generated from (Fig 3).

To alleviate this connectivity issue between two different regions, we propose to use a simple region-specific k-NN search. For samples associated with the manipulation regions, we perform two independent k-NN search: one is performed only with samples associated with , resulting in blue dotted lines in Fig. 3, and the other k-NN is performed only with samples associated with , resulting in the orange dotted lines in Fig. 3, for improving the connectivity between two different regions. For samples associated with the base regions, we simply use the original k-NN search that is performed with all the available samples.

We found that our region-specific k-NN search may generate unnecessary edges between samples that are located in different regions, causing a more memory overhead, approximately 25% over using the original k-NN search. Fortunately, our method is based on LazyPRM [12], and thus there is not much difference in terms of the number of edge collision checking, since we perform edge collision checking on the solution paths. As a result, using region-specific k-NN search improves the path costs by 7% in our tested environments without a significant runtime performance overhead, thanks to better connectivity between two different regions (Table II).

Iv-D Distance Metric

We perform planning in the joint C-space consisting of the base and manipulator. Since the base and joints have different kinds of quantities, i.e., linear and angular quantities, we have the incompatibility issue, which can lower the efficacy of performing nearest neighbor search during planning with respect to the asymptotic optimality [15].

In case of the incompatibility, LaValle et al. [20] considered a robot displacement by using coefficients of different quantities as weights, , to match the units, as the following:


where is the DoF of the joint C-space; for the mobile Hubo robot, correspond to the base configuration, and the rest to the manipulator.

We explain how to determine weights to match the units using the robot displacement. For two configurations and , the robot displacement metric, , can be defined as:


where is the position of point for the robot with the configuration in the workspace.

Since the robot displacement metric yields the maximum amount in the workspace, the first joint with the largest change of the manipulator should have a larger weight. While fixing weights of to for the simplicity, the maximum displacement for each angular quantity is treated as arc lengths in the workspace that can be made given its joint. One can then show that the weight for a joint of a manipulator can be computed by the distance from the joint to the end-effector; the weight intuitively corresponds to the maximum arc length per radian.

Supporting multi-goal configurations.

Given the target end-effector pose , our work supports multi-goal configurations in the joint C-space by planning the base and manipulator simultaneously with our harmonious sampling. Since possible goal configurations from can be infinite, our method dynamically adds goal configurations by injecting the reachability map and IK solver [2] during planning according to a probability of (line 5-6 of Alg. 1). Our harmonious sampler then generate samples in the joint C-space according to , naturally supporting dynamically added goal configurations.

V Experiments and Analysis

(a) Grasping an object on the table (Problem 1).
(b) Grasping an object on the table with ground obstacles (Problem 2).
(c) Grasping an object on the shelf (Problem 3).
(d) Grasping an object on the shelf with ground obstacles (Problem 4).
(e) The worst case of the predefined posture (Problem 5).
(f) The narrow passage (Problem 6).
Fig. 4: This shows our test scenes with the start and goal configurations, and . and are to grasp an object on the table. and are to grasp an object on the shelf. and consist of additional objects, obstructing the robot. shows a difficult case of the predefined posture in . The manipulator is located under the table. shows the narrow passage where adjusting a manipulator is needed to move a base.

We test our method on a machine that has 3.40GHz Intel i7-6700 CPU and 16GB RAM. We use the simulated mobile Hubo robots for testing our method within the OpenRAVE simulator (Fig. 4). We also use a real mobile Hubo robot [21] for testing the usability in a real environment (Fig. 1). A manipulator of the robot has seven joints, and its base consists of , and . We partition the base space into a grid form using resolution for , and degree resolution for . We use LazyPRM [12] as our base optimal planner, and set and for our tests.

To validate our method, we compare our method against two different approaches: the decoupled and coupled approaches. The decoupled approach [5] divides the planning process into two separate planning for the base and manipulator, while the coupled approach [3] performs the planning within the joint C-space representing both base and manipulators and thus having 10 DoFs.

For the decoupled planning, we need to allocate separate time budget for base and manipulator planning parts for the same-time comparison against other approaches. Unfortunately, this issue was not explicitly discussed in prior techniques, and left for user setting. We test different time budgets for the decoupled method, and found time budget ratio for the base and the manipulator shows the best performance on average. Fig. 5(c) shows results with varying ratios in one of difficult testing environments.

We experiment for various planning with a single goal configuration except for the multiple goal approach generating multiple goal configurations given the target end-effector pose. For all the experiments, we perform tests given a time budget of 100 seconds, and report their average performance.

(a) Result of Problem 3 w/ different methods
(b) Result of Problem 4 w/ different methods
(c) Results w/ different time budget ratios for the decoupled approach in Problem 4
Fig. 5: This figure shows the cost over planning time of different methods. and show that our method (Ours + Multi) achieves the best performance. shows that time budget ratio for the base and the manipulator used in the decoupled approach shows the best performance among the tested budget ratios.We visualize graphs from the time that an initial solution is computed.

V-a Test problems

We prepare six different problems to validate our approach, as shown in Fig. 4. Our test scene is wide. The first and second problems (table scenes) are designed to grasp an object on the table as relatively simpler problems with a few obstacles for the manipulator. The third and fourth problems (shelf scenes) have a narrow shelf with obstacles on the bookshelf. Moreover, to show the effect of base movement, the second and fourth problems additionally place ground obstacles that interfere with the movement of a robot.

To intentionally create a difficult case that we may encounter in practice, we use predefined postures for the fifth and sixth that are difficult for manipulating objects or passing narrow passages without dynamically modifying the predefined postures. For other scenes, we use the basic posture of the mobile Hubo robot.

Decoupled Coupled Ours + Ours +
Approach Approach Single Multi
# of time-outs 0 0 0 0
Problem 1 Initial solution time 5.70 11.52 2.23 2.45
Fig. 4(a) (Base/Manipulator) (0.57/5.13)
Final distance cost 4.61 8.10 4.61 4.03
# of time-outs 0 0 0 0
Problem 2 Initial solution time 6.55 27.72 2.79 3.08
Fig. 4(b) (Base/Manipulator) (0.93/5.62)
Final distance cost 5.40 9.65 5.18 4.81
# of time-outs 11 0 0 0
Problem 3 Initial solution time 39.15 9.25 7.00 4.14
Fig. 4(c) (Base/Manipulator) (0.75/38.40)
Final distance cost 6.92 8.73 5.71 5.62
# of time-outs 12 21 0 0
Problem 4 Initial solution time 41.50 52.74 24.34 30.26
Fig. 4(d) (Base/Manipulator) (2.05/39.45)
Final distance cost 7.24 12.76 6.37 6.23
# of time-outs 0 0 0 0
Problem 5 Initial solution time 9.55 31.96 3.94 3.65
Fig. 4(e) (Base/Manipulator) (2.07/7.48)
Final distance cost 6.97 9.84 5.79 5.08
# of time-outs - 28 0 0
Problem 6 Initial solution time - 70.90 36.91 33.13
Fig. 4(f) (Base/Manipulator)
Final distance cost - 16.45 12.16 11.73
TABLE I: Various statistics, averaged out from 50 independent tests. For the decoupled approach, we also show separate times (s) spent on finding an initial solution for each of the base and the manipulator.

V-B Result Analysis

We compare our method with the coupled and decoupled approaches across six different problems. Initially, our method identifies the manipulation regions and constructs the harmonious sampler (0.5s at the most); these times are included in the reported planning time. Table I shows various run-time results of the tested methods. Problem 1 and 2 are simple with a few obstacles, so all the methods find a solution in reasonable planning time (Table I). In these simple cases, the simplest, decoupled approach finds initial solutions faster than the coupled approach that performs sampling in the joint C-space. For Problem 3, on the other hand, the coupled approach finds solutions faster than the decoupled approach. Furthermore, the decoupled approach did not find solutions in of tests. This clearly indicates the problem of the decoupled approach that does not work well at the narrow space. Overall, the decoupled and coupled approaches have the weakness in terms of showing the robust performance across different scenes.

Fortunately, our methods, especially, Ours + Multi considering multiple goal configurations, find solutions faster than both coupled and decoupled approaches, and achieve the shortest path in all of Problem 1 to 4. Fig. 5(a) and Fig. 5(b) show how different methods behave as a function of planning time. Our method (Ours + Multi) computes shorter paths given any fixed planning time. These results clearly show benefits of our approach that adaptively performs sampling for base and manipulators in their joint C-space, even in simple and middle-level problems.

Let us now consider most difficult problems among the tested ones. In Problem 5, our method finds the initial solution much faster, more than one order of magnitude for Ours + Multi, than the coupled and decoupled approaches. In the case of Problem 6, also, ours achieves better solutions than the coupled approach, even though the decoupled cannot solve the problem. These results demonstrate the efficiency and robustness of our methods.

Single vs. multiple goal configurations. Considering multiple goal configurations shows to produce smaller costs across all the tests given the time budget (100s), thanks to considering various candidate configurations to reach the target end-effector pose (Table I). Nonetheless, ours with multiple goal configurations shows similar performance in terms of finding an initial solution over considering only a single goal configuration. This is because the overhead of considering multiple goal configurations is about 2.8% of the overall planning time, resulting in slower performance for identifying the initial solution. However, this overhead is small, and thus is paid well as we have more planning time for achieving shorter paths.

k-NN type Original Region-specific
Ratio of Ours 0.2Ours Ours (Eq. 1) 5Ours
Problem 2
Fig. 4(b)
# of time-outs 0 0 0 0
Initial solution time 2.99 5.26 2.79 3.08
Final distance cost 5.18 5.45 5.18 5.33
Problem 4
Fig. 4(d)
# of time-outs 4 1 0 1
Initial solution time 31.93 37.19 24.34 29.61
Final distance cost 6.85 6.79 6.37 6.61
TABLE II: The results of the simple problem 2 and the difficult problem 4 with varying volume ratios for our harmonious sampler and region-specific NN search.

Analysis of the harmonious sampler. To see benefits of our harmonious sampler , we check how the performance of our method behaves according to of Eq. 1 for Problem 2 and 4, given the region-specific NN search. Specifically, we compare the performance of our approach (Ours) against decreasing and increasing by five times; they are denoted as 0.2Ours and 5Ours, respectively. Table II shows that the number of time-outs, cost, and planning time are increased when is increased or decreased. These results support the benefit of our harmonious sampler taking into account the hyper-volumes of different regions.

Analysis of the region-specific k-NN search. To see the benefits of our region-specific k-NN search, we compare its performance against the original k-NN search that performs k-NN search with all the samples in the joint C-space. Table II shows that in the simple problem (Problem 2), there is not much difference in terms of the final distance cost between two methods. On the other hand, in a difficult problem (Problem 4), our region-specific k-NN search (Ours) reduces all of three measures (including the final distance cost) over using the original k-NN search. Specifically, using the region-specific k-NN search shows times performance improvement in terms of finding an initial solution time, and cost reduction, compared to using the original k-NN search.

5 Random predefined postures Decoupled Coupled Ours + Ours +
Approach Approach Single Multi
# of time-outs 0 0 0 0
Problem 1 Initial solution time 4.79 8.88 2.91 4.18
Fig. 4(a) (Base/Manipulator) (0.54/4.25)
Final distance cost 6.75 9.04 6.76 5.95
# of time-outs 13 21 1 1
Problem 4 Initial solution time 41.78 55.19 17.04 12.64
Fig. 4(d) (Base/Manipulator) (1.60/40.18)
Final distance cost 9.28 12.79 8.27 7.72
# of time-outs - 29 0 0
Problem 6 Initial solution time - 63.39 42.73 45.57
Fig. 4(f) (Base/Manipulator)
Final distance cost - 16.13 14.43 14.65
TABLE III: Results with a predefined posture, which is chosen from five random postures. These results are averaged out from 50 independent tests.

Analysis with different predefined postures. We also test different methods with five random postures as the predefined posture. Table III shows results in Problem 1, 4, and 6; other scenes show similar tendencies. Our methods find solutions faster than both decoupled and coupled approaches and achieve the shortest paths in all of tested problems even with different initial postures, demonstrating the robustness of our approach. These results show that our method robustly achieves a better performance than both decoupled and coupled approaches.

Fig. 6: This figure shows our experimental environment mimicking a convenient store with the mobile Hubo robot. There are various kinds of beverages and snacks on the shelf, and our goal is to grasp the yellow beverage. Our approach computes a shorter path over the decoupled approach in the same time budget.

V-C Real Robot Test

We integrated our approach with the real, mobile Hubo robot. We tested in an experimental environment designed for mimicking a convenience store with various kinds of beverages and snacks on the shelf (Fig. 6). We defined a scenario that the robot moves to the shelf and grasps the yellow beverage on the shelf (Fig. 1). In this scenario, we compared our method with the decoupled approach. Our method stretched out the manipulator while moving the base body. On the other hand, the decoupled approach moves to the shelf without adjusting the manipulator, and then adjusts the manipulator to grasp the yellow beverage, resulting in an inferior path. As a result, our method grasped the target object with a better solution than the decoupled approach; i.e., the cost of our approach is 3.962, while the decoupled one has 4.318.

Vi Conclusion and future work

In this paper, we have introduced a simple, yet effective mobile manipulation sampling method named harmonious sampling that adaptively adjusts the sampling space for the base and the manipulator. Our harmonious sampling works upon the low-dimensional base space to rapidly explore a wide-open space, while exploring in a high-dimensional space where it is necessary to consider full DoFs of the mobile manipulator. Through the experiments, we have shown the improvement of performance in a variety of indoor environments with practical tasks. Furthermore, we have tested our approach by integrating with the mobile Hubo in the real environment.

In general, it is critical to consider the uncertainty of sensing and robot control. Since our method simultaneously operates for the base and manipulator, it is important to handle the uncertainties, which are under active research in robotics. In future work, we would like to efficiently handle them, and support anytime planning.


We would like to thank anonymous reviewers for constructive comments. This work was supported by MI/KEIT (10070171) and NRF/MSIT (No.2019R1A2C3002833).


  • [1] B. Akgun and M. Stilman (2011) Sampling heuristics for optimal motion planning in high dimensions. In IROS, pp. 2640–2645. Cited by: §II-A, §IV-B.
  • [2] D. Berenson, S. S. Srinivasa, D. Ferguson, A. Collet, and J. J. Kuffner (2009) Manipulation planning with workspace goal regions. In ICRA, pp. 618–624. Cited by: §II-B, §IV-D.
  • [3] F. Burget, M. Bennewitz, and W. Burgard (2016) BIRRT*: an efficient sampling-based path planning framework for task-constrained mobile manipulation. In IROS, pp. 3714–3721. Cited by: §II-A, §V.
  • [4] B. Burns and O. Brock (2005) Toward optimal configuration space sampling.. In RSS, pp. 105–112. Cited by: §II-A.
  • [5] S. Chitta, E. G. Jones, M. Ciocarlie, and K. Hsiao (2012) Perception, planning, and execution for mobile manipulation in unstructured environments. Robotics and Automation Magazine 19 (2), pp. 58–71. Cited by: §I, §II-B, §V.
  • [6] J. Dong and J. C. Trinkle (2015) Orientation-based reachability map for robot base placement. In IROS, pp. 1488–1493. Cited by: §II-B.
  • [7] J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot (2014) Informed RRT*: optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic. In IROS, pp. 2997–3004. Cited by: §II-A.
  • [8] J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot (2015) Batch informed trees (BIT*): sampling-based optimal planning via the heuristically guided search of implicit random geometric graphs. In ICRA, pp. 3067–3074. Cited by: §II-A.
  • [9] C. R. Garrett, T. Lozano-Pérez, and L. P. Kaelbling (2017) Sample-based methods for factored task and motion planning. In RSS, Cited by: §II-B.
  • [10] K. Gochev, A. Safonova, and M. Likhachev (2012) Planning with adaptive dimensionality for mobile manipulation. In ICRA, pp. 2944–2951. Cited by: §II-A.
  • [11] K. Gochev, A. Safonova, and M. Likhachev (2013) Incremental planning with adaptive dimensionality.. In ICAPS, Cited by: §II-A.
  • [12] K. Hauser (2015) Lazy collision checking in asymptotically-optimal motion planning.. In ICRA, pp. 2951–2957. Cited by: §II-A, §III, §IV-C, §V.
  • [13] A. Hertle and B. Nebel (2017) Identifying good poses when doing your household chores: creation and exploitation of inverse surface reachability maps. In IROS, pp. 6053–6058. Cited by: §II-B.
  • [14] L. Janson, E. Schmerling, A. Clark, and M. Pavone (2015) Fast Marching Tree: a fast marching sampling-based method for optimal motion planning in many dimensions. The International Journal of Robotics Research 34 (7), pp. 883–921. Cited by: §II-A.
  • [15] S. Karaman and E. Frazzoli (2011) Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research 30 (7), pp. 846–894. Cited by: §II-A, §IV-D.
  • [16] L. E. Kavraki, P. Svestka, J. Latombe, and M. H. Overmars (1996) Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation 12 (4), pp. 566–580. Cited by: §II-A.
  • [17] S. Kiesel, T. Gu, and W. Ruml (2017) An effort bias for sampling-based motion planning. In IROS, pp. 2864–2871. Cited by: §II-A.
  • [18] D. Kim, J. Lee, and S. Yoon (2014) Cloud RRT∗: sampling cloud based rrt∗. In ICRA, pp. 2519–2526. Cited by: §IV-A.
  • [19] S. M. LaValle (1998) Rapidly-exploring random trees a new tool for path planning. Technical report Technical Report 98-11, Iowa State University. Cited by: §II-A.
  • [20] S. M. LaValle (2006) Planning algorithms. Cambridge university press. Cited by: §IV-D.
  • [21] M. Lee, Y. Heo, J. Park, H. Yang, P. Benz, H. Jang, H. Park, I. Kweon, and J. Oh (2019) Fast perception, planning, and execution for a robotic butler: wheeled humanoid m-hubo. In IROS, Cited by: §V.
  • [22] J. Nasir, F. Islam, U. Malik, Y. Ayaz, O. Hasan, M. Khan, and M. S. Muhammad (2013) RRT*-SMART: a rapid convergence implementation of rrt. International Journal of Advanced Robotic Systems 10 (7), pp. 299. Cited by: §II-A, §IV-B.
  • [23] P. S. Schmitt, W. Neubauer, W. Feiten, K. M. Wurm, G. V. Wichert, and W. Burgard (2017) Optimal, sampling-based manipulation planning. In ICRA, pp. 3426–3432. Cited by: §II-A.
  • [24] S. Srivastava, E. Fang, L. Riano, R. Chitnis, S. Russell, and P. Abbeel (2014) Combined task and motion planning through an extensible planner-independent interface layer. In ICRA, pp. 639–646. Cited by: §II-B.
  • [25] M. Stilman, J. Schamburek, J. Kuffner, and T. Asfour (2007) Manipulation planning among movable obstacles. In ICRA, pp. 3327–3332. Cited by: §II-B.
  • [26] N. Vahrenkamp, T. Asfour, and R. Dillmann (2013) Robot placement based on reachability inversion. In ICRA, pp. 1970–1975. Cited by: §I, §II-B, §IV-A, §IV-A.
  • [27] Y. Yang and O. Brock (2005) Efficient motion planning based on disassembly. In RSS, pp. 97–104. Cited by: §IV-A.
Comments 0
Request Comment
You are adding the first comment!
How to quickly get a good reply:
  • Give credit where it’s due by listing out the positive aspects of a paper before getting into which changes should be made.
  • Be specific in your critique, and provide supporting evidence with appropriate references to substantiate general statements.
  • Your comment should inspire ideas to flow and help the author improves the paper.

The better we are at sharing our knowledge with each other, the faster we move forward.
The feedback must be of minimum 40 characters and the title a minimum of 5 characters
Add comment
Loading ...
This is a comment super asjknd jkasnjk adsnkj
The feedback must be of minumum 40 characters
The feedback must be of minumum 40 characters

You are asking your first question!
How to quickly get a good answer:
  • Keep your question short and to the point
  • Check for grammar or spelling errors.
  • Phrase it like a question
Test description