I Introduction
Generating optimal motions plans is crucial for almost any robotic tasks ranging from typical manipulation tasks such as binpicking to autonomous navigation of mobile robots. To solve such tasks, the robotics community relies on two powerful motion planning frameworks: Samplingbased planners and trajectory optimization.
Samplingbased planners like RRT* [13], BIT* [5] or FMT* [11] converge asymptotically to optimal solutions and almost surely provide a solution if one exists [18]. However, these planners are slow at converging to the optimal trajectory, because improvements to the current best solution only arise when we sample a state nearby [28], and often provide nonsmooth trajectories that may require postprocessing [7].
Trajectory optimization methods like KOMO [32], CHOMP [27], STOMP [12] and TrajOpt [29] use optimization methods and can exploit gradient and second order information to converge to a local optimal solution. These optimizationbased methods are typically fast at converging to the local optimum, however, due to the nonconvexity of the problem, the optimizer might converge to a locally optimal or even an infeasible trajectory. These methods therefore do not have convergence guarantees, i.e. they may not converge to a solution even if one exists, and the feasibility of the solution often depends heavily on the initial trajectory [21, 19, 10]. Hence, these methods usually work well in environments with few obstacles or when provided with good initial guesses [20], e.g. for postprocessing paths produced by samplingbased planners.
In order to combine the benefits of both frameworks, we propose to integrate the asymptotically optimal Batch Informed Trees (BIT*) [5] planner with KOrder Markov Optimization (KOMO) [32, 33]. Combining sampling and optimization helps us play on the strengths of each framework and mitigate their weaknesses. Our novel algorithm, BITKOMO, uses BIT* to iteratively generate nonoptimal initial paths that are then optimized by KOMO, which results in quick convergence to local minima. The cost of the optimized path is then used by BIT* to carry a more informed search. With this method, we maintain the asymptotic global optimality guarantees by BIT* while also benefiting from the fast convergence of KOMO (Fig. 1).
At the core of BITKOMO lies a new relaxed edge collision checking method. Relaxed edge collision is an intermediate approach between full and lazy [9] collision checking, where we allow partiallyvalid edges to remain, because the trajectory optimizer KOMO can often push invalid paths out of collision [33] to converge to feasible solutions. Even though this modification allows for invalid edges, we do not sacrifice any of the asymptotic guarantees provided by BIT*.
To summarize, we make two major contributions:

Relaxed edge collision checking: A method for BIT* that allows edges partially in collision to be included in the motion tree.

BITKOMO: A planner integrating BIT* and KOMO. We integrate sampling and optimization to obtain fast convergence to the global optimum while maintaining the guarantees provided by the sampler.
Ii Related Work
Combining both asymptoticallyoptimal planners [14, 28, 6, 30] and trajectory optimization [32, 27, 12, 29, 22] into one concise algorithm is an important longterm goal of the robotics community [8]. There exist three main approaches. The twostep approach, where we run a samplingbased planner like the rapidlyexploring random tree (RRT) [18], or the probabilistic roadmap (PRM) [15] until a valid solution path is found. In a postprocessing step [16, 7], we call a trajectory optimizer which optimizes the path to find a (local) optimal solution. This method is the most common approach, but does not hold guarantees on optimality, and does not allow the samplingbased planner to find improved solutions by leveraging information acquired by the optimizer. Our BITKOMO approach improves upon this methodology by tightly integrating sampling and optimization in an iterative fashion. Moreover, we use relaxed collision checking to quickly find partiallyvalid paths. Optimizers like KOMO [32] can often repair those paths to quickly find a solution.
The second approach is the optimizerassteering method, where the integration of sampling and optimization is done using the steering function from the planner. The regionallyaccelerated BIT* (RABIT*) [2] is an extension of BIT* that uses an optimizer to push infeasible edges out of collision. However, while the obtained solution cost is often better than BIT*, calls to an optimizer are often expensive and slow down the algorithm. It is also possible to integrate such an approach into a roadmap planner [1], to interleave edge creation and optimization steps. We differ by postponing optimization to the point where a full partiallyvalid path has been found, which reduces the number of unnecessary calls to the nonlinear optimizer.
Finally, the pathproposal method, where the planner proposes solution paths, which are then send to the optimizer [17]. An important aspect of this method is to propose many diverse solution paths, so that the optimizer has a lesser change to converge to similar solutions. This can be accomplished by leveraging sparse roadmaps [23], to generate diverse initial trajectories which we can then send to the trajectory optimizer [3, 25, 24]. The closest work to ours is [8], where RRT* is integrated with a nonlinear optimizer in an iterative fashion.
Our method is complementary, in that we also use the pathproposal method. However, we differ from previous works by our choice of the underlying components (BIT* and KOMO) and/or the interface. Further, the novel collision checking improves the performance in problems involving narrow passages, when the samplingbased planner fails to find a path in the first place.
Iii Problem Definition
We consider a motion planning problem in a configuration space of the form () where is the free configuration space, is the start configuration, is the goal configuration and is the cost functional mapping a trajectory in the free configuration space to a real number. Our goal is to find a trajectory from , to that is optimal, i.e. the value of is the lowest among all possible paths.
(1a)  
s.t.  (1b)  
(1c) 
In this paper, we focus on pathlength optimization, i.e. . Typical examples of alternative cost functionals are the sum of the velocities squared , and smoothness .
Iv Bitkomo
We develop BITKOMO by integrating BIT* with KOMO. Our planner maintains the asymptotic optimality guarantees of BIT* while converging faster to the global minimum by leveraging trajectory optimization. Because KOMO uses the Augmented Lagrangian algorithm for constrained optimization, it can sometimes push partially invalid paths out of collision. To exploit this feature, we introduce relaxed edge checking, which allows BIT* to produce partially infeasible paths for subsequent optimization with KOMO when necessary.
Iva The BITKOMO Algorithm: An overview
Fig. 2 gives an overview of our planner. Our planner requires a valid start state (), a goal state () and the full information about the environment (). We also need to provide the Planner Termination condition () and the edge relaxation constant (). To begin, the BIT* planner samples a batch of configurations and builds an edgeimplicit Random Geometric Graph (RGG) [26] [Block A in Fig. 2]. The best edge that can possibly improve the cost to goal is then chosen and passed to the Relaxed Edge Checker [B] to carry out the collision checking. The Relaxed Edge Checker performs a validity check and returns the collision penalty (), an integer that provides a proxy measure for the fraction of the edge that is in collision. The planner uses this integer to decide regarding the addition of the edge to the tree. If an improved path to the goal is found, it is passed to the KOMO optimizer [D] which locally optimizes the path and, if valid, returns the new cost to the BIT* planner. BIT* uses this path cost to prune the unnecessary vertices and edges and carry out a more focused search. When no new edges can be expanded, the Sample function [C] is called, which adds another batch of samples to the RGG.
Algorithm 1 describes in more detail the different parts of the planner. The highlighted lines are our addition to the BIT* planner. Color blue corresponds to the Relaxed edge collision checking and color orange corresponds to the interface between BIT* and KOMO.
IvA1 Initialize (A)
Initialize the vertex set (), the edge set () and the tree () along with the vertex queue (), the edge queue () and the set of unconnected vertices (). We also initialize three important cost parameters: 1) is used by the BIT* tree and also includes partially infeasible edges, 2) is the cost of the best feasible path, and 3) is a penalty cost higher than any feasible path BIT* could converge to.
IvA2 Batch Addition (B)
This is called if we run out of edges that can help improve our solution. We then add more vertices to our implicit graph and also prune vertices and edges that cannot improve our solution. A detailed explanation can be found in [4].
IvA3 Edge Selection (C)
Checks if expanding any vertex can help us in improving the cost of our solution. If the expansion of a vertex can possibly improve the cost, it is expanded, i.e., relevant vertices and edges are added to the vertex queue () and edge queue () respectively (line 12)
IvA4 Edge processing (D)
Decides whether to add the edge to the tree. We first check (line 15) if the new edge assuming collisionfree, could improve the overall cost to goal and the cost to go to the new vertex . If yes, then we check the edge for collision (line 16). If the collision penalty is less than our relaxation constant (line 17), we calculate the edge cost as the sum of the collisionfree edge () and the collision penalty cost () (line 18) and check if the new edge could actually improve the cost to go (line 19). If yes, then we add the new edge to our tree (line 20). (line 20) function also rewires the tree if necessary.
IvA5 KOMO Optimization (E)
If the new edge added to the tree improves the cost to the goal, we optimize it with KOMO (line 26). If the resulting path () is valid, we update the cost () for BIT* to continue in a more informed manner (line 28). For completeness, we also check if the initial guess is valid by checking if the path cost is less than (line 22) and update if valid. The working of KOMO is explained in IVC.
IvB Relaxed Edge Checking
High dimensional spaces containing narrow passages are challenging for sampling based planners. This is because it is difficult to sample collision free edges through narrow passages. Since KOMO can push paths out of obstacles, we could allow paths partially in collision into the BIT* tree. However, these edges need to be added with sufficient collision penalty, otherwise, we may never get an initial feasible solution, e.g. if a partially infeasible straight line path is added to BIT* and KOMO cannot push it out of collision, BIT* will never explore further. We require our planner to prefer paths that are collision free over paths that are in collision for optimization as the likelihood of finding a feasible trajectory from a collisionfree path is higher. We solve this problem by introducing Relaxed Edge Checking which returns a number instead of a boolean which is used to assign a collision penalty (line 18). It returns 0if edge is collision free, 1if it fails at the last level, 2if it fails on the second to last level, and so on.
The Relaxed Edge Checker divides the path into equally spaced points depending on the length and the desired resolution. The collision checking is then conducted level wise (see Fig. 3) so that we increase the resolution of our checking until we have reached the required resolution. We first check the mid point (level 1), then the quarter points (level 2) and so on slowly doubling the resolution of checking. If a point fails in the validity check, an integer, collision penalty () is returned. Where is the total number of levels, and is the level of the failed point. This number provides a proxy measure for the fraction of the edge that is in collision.
IvC Komo
KOrder Markov Optimization (KOMO) is a trajectory optimization framework that represents a path with a discrete sequence of waypoints . Cost and constraints are evaluated on up to consecutive waypoints (Markov assumption)
(2a)  
s.t.  (2b) 
where is a tuple of consecutive states. In our setting, where the goal is to minimize the path length, , and we use as cost the sum of squared distances , which corresponds to .
Inequality constraints correspond to collision avoidance and joint limits and equality constraints model the terminal goal condition . The optimization problem (2) is solved with the Augmented Lagrangian algorithm for constrained optimization. The Markov structure, together with second order information, enables very efficient solving, with complexity linear on the number of waypoints and polynomial on the dimension of the configuration space [32].
IvD Convergence and Optimality Guarantees
BITKOMO maintains the convergence and optimality guarantees of BIT* [5]. The additional trajectory optimization can only improve the solution proposed by BIT* (lines 2630 in Alg. 1). The relaxed edge checking assigns cost (line 18 in Alg. 1) to any edge in collision (recall that is an upper bound on the optimal solution cost, that can be chosen arbitrarily large). Even if the subsequent optimization fails, the edge cost does not prevent BIT* and hence BITKOMO from finding a solution with cost .
V Evaluation
Va Scenarios
We evaluate our algorithm on 6 different robotic scenarios^{1}^{1}1The results can be verified using the our code available on GITHUB. In all scenarios, the robot moves from the initial configuration (solid color) to the goal configuration (translucent color) (Fig. 4). The trajectories computed by BITKOMO and the baseline algorithms are shown in the supplementary video. We emphasize the challenges of each problem with the keywords: narrow passage,
not informative heuristic
and highdimensional.
Disc Robot in Rooms: A Disc Robot needs to move from the center of one room to another (Fig. 3(a)). The difficulty is that, to go to the other room, the robot first needs to come out of the starting room and then move to the target room. Challenges: narrow passages and not informative heuristic.

Kuka to reach onto the shelf: The Kuka robot needs to reach the red object at level 1 from it’s current position where the endeffector is at level 2 (Fig.3(b)). Challenges: Highdimensional.

Kuka to reach into a box: The Kuka Robot needs to reach to the object inside the box that is located under a table while avoiding collision with the table or the box (Fig. 3(c)). Challenges: Narrow Passage, High Dimensional.

Two Fixed Pandas: The robotic manipulators (Pandas) need to get to the base of the opposite robot while avoiding hitting each other (Fig. 3(d)). Challenges: High Dimensional.

Two Mobile Pandas in cluttered environment: Two mobile panda robots need to move to the other side of the room while avoiding obstacles and also avoiding each other (Fig. 3(e)). Challenges: High Dimensional, Narrow Passages.

One Mobile Panda to avoid large obstacle: The mobile panda needs to move to catch an object on the other side of the scene, with a large obstacle blocking it’s way (Fig. 3(f)). Challenges: High Dimensional, Narrow Passages.
VB Baselines
We compare our BITKOMO planner with BIT* [5], KOMO [32] and FMT* [11]. Path length minimization is used as the optimization objective for all the experiments. We used the Open Motion Planning Library (OMPL) [31] framework for the implementations of the sampling based planners and for carrying out the benchmarks.
For the KOMO planner we use the sum of squares of the distances between waypoints as optimization objective, and an initialization with random noise around . The trajectory is represented with a constant number of waypoints (20 points). Random initialization and optimization are executed iteratively until timeout, updating the path cost when a better path is found.
VC Metrics
We evaluate the planners on 2 different metrics:

Success Rate (%): The % of runs that have found a feasible solution at time . This metric gives information about how fast the planner finds the first feasible path.

Cost: The average best cost of the planner at time . This metric gives us an understanding about how the best cost solution of a planner evolves over time and the practical convergence speed before the timeout.
VD Experimental Results
For getting unbiased results, all experiments were carried out on the same machine^{2}^{2}2Intel(R) Core(TM) i56200U CPU @ 2.30GHz having 16GB RAM. Every planner was executed 50 times on all the six example scenarios. The maximum execution time however was different for different scenarios depending on the difficulty. The relaxation number was set to 1 for all examples. The results of the benchmarks are shown in Fig. 5.
VD1 Success Rate
The success rate of BIT* and BIKOMO were higher compared with other planners in all examples except the Two Mobile Pandas example. This scenario has narrow passages which makes it hard for sampling based planners, however, the optimal solution is very similar to a straight line path in the configurations space, making it very easy for KOMO to find a solution here. The relaxed edge checking helps BITKOMO in having a slightly better success rate than BIT* here. Overall, the success rates of BIT* and BITKOMO were found to be very similar because they generate initial paths using the same base algorithm.
VD2 Cost
BITKOMO decreases the cost significantly faster than BIT*, with better convergence before the timeout. This is because the combination of sampling and optimization converges to the local minima quickly and consistently. We however see an abnormality in Fig.4(a), this is because 1) KOMO is not much faster than sampling for low dimensions, and 2) The waypoints maintain a certain minimum distance from the obstacles to avoid edge collisions.
Overall, we conclude that our planner is mostly as good as BIT* in finding the first feasible solution and slightly faster in high dimensional narrow passage problems, but much faster at converging to the global optimal solution.
Vi Conclusion
Our planner, BITKOMO, combines BIT* and KOMO to achieve fast convergence to the optimal solution while being anytime and asymptotically converging to the global minimum. Our experiments indicate that BITKOMO converges to the global optima, faster than BIT*. BITKOMO also provides convergence guarantees which KOMO fails to provide. Our planner, using Relaxed Edge Checking, exploits the ability of KOMO to move trajectories away from the obstacles that are in collision by allowing partially infeasible paths as initial guesses to the optimizer. This helps BITKOMO find motions through narrow passages faster than BIT*.
Even though we observe faster convergence than BIT* to optimal paths, our planner does not have a better success rate. A dedicated planner could be developed to generate improved initial guesses to the optimizer, resulting in an increased success rate. Our method also depends on the number of waypoints KOMO uses. Too many or too few could hamper the performance of the planner. Future work could be done on automatically choosing the optimal number of waypoints for a given problem. The optimization and sampling modules could easily be parallelized, providing a higher speedup.
Our experiments clearly demonstrate that BITKOMO can robustly achieve fast convergence to optimal motion plans. This is an important step towards making optimal motion planners converge as quickly as trajectory optimizers—all while keeping asymptotic optimality guarantees.
References
 [1] (2020) Joint sampling and trajectory optimization over graphs for online motion planning. In IEEE International Conference on Intelligent Robots and Systems, pp. 4700–4707. Cited by: §II.
 [2] (2016) Regionally accelerated batch informed trees (rabit*): a framework to integrate local information into optimal path planning. In 2016 IEEE International Conference on Robotics and Automation (ICRA), pp. 4207–4214. Cited by: §II.
 [3] (2018) Improving trajectory optimization using a roadmap framework. In 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 8674–8681. Cited by: §II.
 [4] (2020) Batch informed trees (bit*): informed asymptotically optimal anytime search. The International Journal of Robotics Research 39 (5), pp. 543–567. Cited by: §IVA2.
 [5] (2015) Batch informed trees (BIT*): samplingbased optimal planning via the heuristically guided search of implicit random geometric graphs. In 2015 IEEE international conference on robotics and automation (ICRA), pp. 3067–3074. Cited by: §I, §I, §IVD, §VB.
 [6] (2021) Asymptotically optimal samplingbased motion planning methods. Annual Review of Control, Robotics, and Autonomous Systems 4, pp. 295–318. Cited by: §II.
 [7] (2007) Creating highquality paths for motion planning. The international journal of robotics research 26 (8), pp. 845–863. Cited by: §I, §II.
 [8] (2020) Planning planning: the path planner as a finite state machine. Cited by: §II, §II.
 [9] (2015) Lazy collision checking in asymptoticallyoptimal motion planning. In IEEE International Conference on Robotics and Automation, pp. 2951–2957. Cited by: §I.
 [10] (2020) Deep learning can accelerate graspoptimized motion planning. Science Robotics 5 (48), pp. eabd7710. Cited by: §I.
 [11] (2015) Fast marching tree: a fast marching samplingbased method for optimal motion planning in many dimensions. The International journal of robotics research 34 (7), pp. 883–921. Cited by: §I, §VB.
 [12] (2011) STOMP: stochastic trajectory optimization for motion planning. In 2011 IEEE international conference on robotics and automation, pp. 4569–4574. Cited by: §I, §II.
 [13] (2010) Optimal kinodynamic motion planning using incremental samplingbased methods. In 49th IEEE conference on decision and control (CDC), pp. 7681–7687. Cited by: §I.
 [14] (2011) Samplingbased algorithms for optimal motion planning. International Journal of Robotics Research 30 (7), pp. 846–894. Cited by: §II.
 [15] (1996) Probabilistic roadmaps for path planning in highdimensional configuration spaces. IEEE transactions on Robotics and Automation 12 (4), pp. 566–580. Cited by: §II.
 [16] (2003) Extracting optimal paths from roadmaps for motion planning. In IEEE International Conference on Robotics and Automation, Vol. 2, pp. 2424–2429. Cited by: §II.
 [17] (2020) Fast anytime motion planning in point clouds by interleaving sampling and interior point optimization. In Robotics Research, pp. 929–945. Cited by: §II.
 [18] (2006) Planning Algorithms. Cambridge University Press. Cited by: §I, §II.
 [19] (2020) Memory of motion for warmstarting trajectory optimization. IEEE Robotics and Automation Letters 5 (2), pp. 2594–2601. Cited by: §I.
 [20] (2017) Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3d complex environments. IEEE Robotics and Automation Letters 2 (3), pp. 1688–1695. Cited by: §I.
 [21] (2018) Leveraging precomputation with problem encoding for warmstarting trajectory optimization in complex environments. In IEEE International Conference on Intelligent Robots and Systems, pp. 5877–5884. Cited by: §I.
 [22] (2018) Continuoustime gaussian process motion planning via probabilistic inference. International Journal of Robotics Research 37 (11), pp. 1319–1340. Cited by: §II.
 [23] (202004) Motion planning explorer: visualizing local minima using a localminima tree. IEEE Robotics and Automation Letters 5 (2), pp. 346–353. External Links: Document Cited by: §II.
 [24] (2020) Visualizing local minima in multirobot motion planning using multilevel morse theory. In Workshop on the Algorithmic Foundations of Robotics, Cited by: §II.
 [25] (2015) Parallel cartesian planning in dynamic environments using constrained trajectory planning. In 2015 IEEERAS 15th International Conference on Humanoid Robots (Humanoids), pp. 983–990. Cited by: §II.
 [26] (2003) Random geometric graphs. Vol. 5, OUP Oxford. Cited by: §IVA.
 [27] (2009) CHOMP: gradient optimization techniques for efficient motion planning. In 2009 IEEE International Conference on Robotics and Automation, pp. 489–494. Cited by: §I, §II.
 [28] (2016) Asymptotically nearoptimal rrt for fast, highquality motion planning. IEEE Transactions on Robotics 32 (3), pp. 473–483. Cited by: §I, §II.
 [29] (2014) Motion planning with sequential convex optimization and convex collision checking. The International Journal of Robotics Research 33 (9), pp. 1251–1270. Cited by: §I, §II.
 [30] (2021) AIT* and EIT*: asymmetric bidirectional samplingbased path planning. arXiv preprint arXiv:2111.01877. Cited by: §II.
 [31] (2012) The open motion planning library. IEEE Robotics & Automation Magazine 19 (4), pp. 72–82. Cited by: §VB.
 [32] (2014) Newton methods for korder markov constrained motion problems. arXiv preprint arXiv:1407.0414. Cited by: §I, §I, §II, §IVC, §VB.

[33]
(2015)
Logicgeometric programming: an optimizationbased approach to combined task and motion planning.
In
International Joint Conference on Artificial Intelligence
, Cited by: §I, §I.