Soft Subdivision Motion Planning for Complex Planar Robots

06/14/2019
by   Bo Zhou, et al.
NYU college
0

The design and implementation of theoretically-sound robot motion planning algorithms is challenging. Within the framework of resolution-exact algorithms, it is possible to exploit soft predicates for collision detection. The design of soft predicates is a balancing act between easily implementable predicates and their accuracy/effectivity. In this paper, we focus on the class of planar polygonal rigid robots with arbitrarily complex geometry. We exploit the remarkable decomposability property of soft collision-detection predicates of such robots. We introduce a general technique to produce such a decomposition. If the robot is an m-gon, the complexity of this approach scales linearly in m. This contrasts with the O(m^3) complexity known for exact planners. It follows that we can now routinely produce soft predicates for any rigid polygonal robot. This results in resolution-exact planners for such robots within the general Soft Subdivision Search (SSS) framework. This is a significant advancement in the theory of sound and complete planners for planar robots. We implemented such decomposed predicates in our open-source Core Library. The experiments show that our algorithms are effective, perform in real time on non-trivial environments, and can outperform many sampling-based methods.

READ FULL TEXT VIEW PDF
POST COMMENT

Comments

There are no comments yet.

Authors

page 1

page 2

page 3

page 4

06/19/2018

Motion planning in high-dimensional spaces

Motion planning is a key tool that allows robots to navigate through an ...
03/22/2019

Rods and Rings: Soft Subdivision Planner for R^3 x S^2

We consider path planning for a rigid spatial robot moving amidst polyhe...
06/30/2020

Predicting Sample Collision with Neural Networks

Many state-of-art robotics applications require fast and efficient motio...
06/15/2021

Constrained Motion Planning of A Cable-Driven Soft Robot With Compressible Curvature Modeling

A cable-driven soft-bodied robot with redundancy can conduct the traject...
11/07/2017

Intelligent Collision Management in Dynamic Environments for Human-Centered Robots

In this context, a major focus of this thesis is on unintentional collis...
03/31/2014

Implementation of interaction between soft tissues and foreign bodies using modified voxel model

Interactive bodies collision detection and elimination is one of the mos...
03/07/2021

When Being Soft Makes You Tough: A Collision Resilient Quadcopter Inspired by Arthropod Exoskeletons

Flying robots are usually rather delicate, and require protective enclos...
This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

1 Introduction

Motion planning is widely studied in robotics latombe:robot-motion:bk ; lavalle:planning:bk ; choset-etal:bk

. Many planners are heuristic, i.e., without a priori guarantees of their performance (see below for what we mean by guarantees). In this paper, we are interested in non-heuristic algorithms for the basic planning problem: this basic problem considers only kinematics and the existence of paths. The robot

is fixed, and the input is a triple where are the start and goal configurations of , and is a polyhedral environment in or . The algorithm outputs an -avoiding path from to if one exists, and NO-PATH otherwise. See robot for some rigid robots, and also gui for our GUI interface for path planning.

Figure 1: Some rigid planar robots ((a)-(b): star-shaped; (c)-(e): general shaped).
Figure 2: GUI interface for planner for a 3-legged robot.

The basic planning problem ignores issues such as the optimality of paths, robot dynamics, planning in the time dimension, non-holonomic constraints, and other considerations of a real scenario. Despite such an idealization, the solution to this basic planning problem is often useful as the basis for finding solutions that do take into account the omitted considerations. E.g., given a kinematic path, we can plan a smooth trajectory with a homotopic trace.

The algorithms for this basic problem are called “planners.” In theory, it is possible to design exact planners because the basic path planning is a semi-algebraic (non-transcendental) problem. Even when such algorithms are available, exact planners have relatively high complexity and are non-adaptive, even in the plane (see milen-sacks-trac:plane-planner:13 ). So we tend to see inexact implementations of exact algorithms, with unclear guarantees. Cristian Calude, editor. The Human Face of Computing, chapter Kurt Mehlhorn: From Theory to Library of Efficient Data Types and Algorithms (LEDA) and Algorithm Engineering, pages 59-72. Imperical College Press, 2015 When fully explicit algorithms are known, exact implementation of exact planners is possible using suitable software tools such as the  library halperin-fogel-wein:bk .

In current robotics lavalle:planning:bk ; choset-etal:bk

, those algorithms that are considered practical and have some guarantees may be classified as either resolution-based or sampling-based. The guarantees for the former is the notion of resolution completeness and for the latter, sampling completeness. Roughly speaking,

if there exists a path then:
– resolution completeness says that a path will be found if the resolution is fine enough;

– sampling completeness says that a path will be found with high probability if “enough” random samples are taken.


But notice that if there is no path, these criteria are silent; indeed, such algorithms would not halt except by artificial cut-offs. Thus a major effort in the last 20 years of sampling research has been devoted to the so-called “Narrow Passage” problem. It is possible to view this problem as a manifestation of the Halting Problem for the sampling approaches: how can the algorithm halt when there is no path? (A possible approach to address this problem might be to combine sampling with exact computation, as in Halperin-ESA11 .)

Motivated by such issues, as well as trying to avoid the need for exact computation, we in wang-chiang-yap:motion-planning:15 ; yap:sss:13 introduced the following replacement for resolution complete planners: a resolution-exact planner takes an extra input parameter in addition to , and it always halts and outputs either an -avoiding path from to or NO-PATH. The output satisfies this condition: there is a constant depending on the planner, but independent of the inputs, such that:
– if there is a path of clearance , it must output a path;
– if there is no path of clearance , it must output NO-PATH.
Notice that if the optimal clearance lies between and , then the algorithm may output either a path or NO-PATH. So there is output indeterminacy. Note that the traditional way of using is to fix , killing off indeterminacy. Unfortunately, this also leads us right back to exact computation which we had wanted to avoid. We believe that indeterminacy is a small price to pay in exchange for avoiding exact computation wang-chiang-yap:motion-planning:15

. The practical efficiency of resolution-exact algorithms is demonstrated by implementations of planar robots with 2, 3 and 4 degrees of freedom (DOF)

wang-chiang-yap:motion-planning:15 ; luo-chiang-lien-yap:link:14 ; yap-luo-hsu:thicklink:16 , and also 5-DOF spatial robots hsu-chiang-yap:rod-ring:19 . All these robots perform in real-time in non-trivial environments. In view of the much stronger guarantees of performance, resolution-exact algorithms might reasonably be expected to have a lower efficiency compared to sampling algorithms. Surprisingly, no such trade-offs were observed: resolution-exact algorithms consistently outperform sampling algorithms. Our 2-link robot luo-chiang-lien-yap:link:14 ; yap-luo-hsu:thicklink:16 was further generalized to have thickness (a feat that exact methods cannot easily duplicate), and can satisfy a non-self-crossing constraint, all without any appreciable slowdown. Finally, these planners are more general than the basic problem: they all work for parametrized families of robots, where ’s are robot parameters. All these suggest the great promise of our approach.

What is New in This Paper. In theoretical path planning, the algorithms often considered simple robots like discs or line segments. In this paper, we consider robots of complex shape, which are more realistic models for real-world robots. We call them “complex robots” (where the complexity comes from the robot geometry rather than from the degrees of freedom). We focus on planar robots that are rigid and connected. Such a robot can be represented by a compact connected polygonal set whose boundary is an -sided polygon, i.e., an -gon. Informally, we call a “complex robot” if it is a non-convex -gon for “moderately large” values of , say . By this criterion, all the robots in robot are “complex.” According to zhang-kim-manocha:path-non-existence:08 , no exact algorithms for have been implemented; in this paper, we have robots with . To see why complex robots may be challenging, recall that the free space of such robots may have complexity (see avnaim-boissonnat-faverjob:practical-polygonal:89 ) when the robot and environment have complexity and , respectively. Even with fixed, this can render the algorithm impractical. For instance, if , the algorithm may slow down by 3 orders of magnitude. But our subdivision approach does not have to compute the entire free space before planning a path; hence the worst-case cubic complexity of the free space is not necessarily an issue.

More importantly, we show that the complexity of our new method grows only linearly with . To achieve this, we exploit a remarkable property of soft predicates called “decomposability.” We show how an arbitrary complex robot can be decomposed (via triangulation that may introduce new vertices) into an ensemble of “nice triangles” for which soft predicates are easy to implement. As we see below, there is a significant difference between a single triangle and an ensemble of triangles. In wang-chiang-yap:motion-planning:15 , they choose the circumcenter of the triangle as the origin of the coordinate system to simplify the predicates. But for us, all the triangles in the ensemble must share the same coordinate system. This complicates our predicates. In consequence of our new techniques, we can now routinely construct resolution-exact planners for any reasonably complex robot provided by a user. This could lead to a flowering of experimentation algorithmics in this subfield.

Technically, it is important to note that the previous soft predicate construction for a triangle robot in wang-chiang-yap:motion-planning:15 ; sss2 requires that the rotation center, i.e., the origin of the (rotational) coordinate system, be chosen to be the circumcenter of the triangle. But for our new soft predicates the triangles in the triangulation of the complex robot cannot be treated in the same way. This is because all the triangles of the triangulation must share a common origin, to serve as the rotation center of the robot. To ensure easy-to-compute predicates, we introduce the notion of a “nice triangulation” relative to a chosen origin: all triangles must be “nice” relative to this origin. These ideas apply for arbitrary complex robots, but we also exploit the special case of star-shaped robots to achieve stronger results.

gui shows our experimental setup for complex robots. A demo showing the real-time performance of our algorithms is found in the video clip available through this web link: https://cs.nyu.edu/exact/gallery/complex/complex-robot-demo.mp4.

Remark. Although it is not our immediate concern to address noisy environments and uncertainties, it is clear that our work can be leveraged to address these issues. E.g., users can choose to be correlated with the uncertainty in the environment and the precision of the robot sensors. By using weighted Voronoi diagrams bennett-papadopoulou-yap:minimization:16 , we can achieve practical planners that have obstacle-dependent clearances (larger clearance for “dangerous” obstacles).

this means that exact computation is possible sharma-yap:crc. The direct representation of algebraic numbers is impractical, but implicit representation in such systems as  core2 or in  halperin-fogel-wein:bk may be used. But of the planners that do have guarantees, success.

Theoretically-sound planners have been designed as far back as the 1980s. But in the rare cases when these exact planners are implemented, the implementation are not guaranteed to be correct because of numerical issues. The exceptions are those algorithms implemented using the principles of Exact Geometric Computation such as in CGAL halperin-fogel-wein:bk . We introduced the notion of resolution-exactness in wang-chiang-yap:motion-planning:15 to side-step the need for exact computation: purely numerical approximation such as provided in BigNumber packages suffices to produce correct implementations.

In this paper, we address another gap between theory and implementations. Exact theoretical algorithms for planar rigid robots has been restricted to “simple” robots, e.g., (rod xxx), (triangle xxx; boissonnat) or (cross xxx). In practice, planners for complex robots have been designed and implemented (e.g., the gear robot manocha). In this paper, we demonstrate techniques for achieving resolution-exact planners for complex robots such as complex.

Previous Related Work. An early work is Zhu-Latombe zhu-latombe:hierarchical:91 who also classify boxes into  or  or  (using our terminology below). They introduced the concept of M-channels (comprised of  or  leaf boxes), as a heuristic basis to find an F-channel comprising only of  boxes. Subsequent researchers (Barbehenn-Hutchinson barbehenn-hutchinson:single-source:95 and Zhang-Manocha-Kim zhang-kim-manocha:path-non-existence:08 ) continued this approach. Researchers in resolution-based approaches were interested in detecting the non-existence of paths, but their solutions remain partial because they do not guarantee to always detect non-existence of paths (of sufficient clearances) basch+3:disconnection:01 ; zhang-kim-manocha:path-non-existence:08 . The challenge of complex robots was taken up by Manocha’s group who implemented a series of such examples zhang-kim-manocha:path-non-existence:08 : a “five-gear” robot, a “2-D puzzle” robot a certain “star” robot with 4 DOFs, and a “serial link” robot with 4 DOFs. Except for the “star,” the rest are planar robots.

Overview of the Paper. Section 2 reviews the fundamentals of our soft subdivision approach. Sections 3 and 4 describe our new techniques for star-shaped robots and for general complex robots, respectively. We present the experimental results in Section 5, and conclude in Section 6. All proofs are put in the appendix at the end of the paper. The conference version of this paper appeared in zhou-chiang-yap:complex-robot:18 .

2 Review: Fundamentals of Soft Subdivision Approach

Our soft subdivision approach includes the following three fundamental concepts (see wang-chiang-yap:motion-planning:15 and the Appendix of luo-chiang-lien-yap:link:14 for the details):

Resolution-exactness. This is an alternative replacement for the standard concept of “resolution completeness” in the subdivision literature. Briefly, a planner is resolution-exact if there is a constant such that if there is a path of clearance , it will return a path, and if there is no path of clearance , it will return NO-PATH. Here, is an additional input to the planner, in addition to the normal parameters.

Soft Predicates. Let be the set of closed axes-aligned boxes in . We are interested in predicates that classify boxes. Let be an (exact) predicate where are called definite values, and the indefinite value. For motion planning, we may also identify with //, respectively. In our application, if is a free configuration, then ; if is on the boundary of the free space, ; otherwise . We extend to boxes as follows: for a definite value , if for every . Otherwise, . Call a “soft version” of if whenever is a definite value, , and moreover, if for any sequence of boxes () that converges monotonically to a point , for large enough.

Soft Subdivision Search (SSS) Framework. This is a general framework for a broad class of motion planning algorithms. One must supply a small number of subroutines with fairly general properties in order to derive a specific algorithm. For SSS, we need a predicate to classify boxes in the configuration space as //, a method to split boxes, a method to test if two  boxes are connected by a path of  boxes, and a method to pick  boxes for splitting. The power of such frameworks is that we can explore a great variety of techniques and strategies. Indeed we introduced the SSS framework to emulate such properties found in the sampling framework.

Feature-Based Approach. Following our previous work  wang-chiang-yap:motion-planning:15 ; luo-chiang-lien-yap:link:14 , our computation and predicates are ”feature based” whereby the evaluations of box primitives are based on a set of features associated with the box . Given a polygonal set of obstacles, the boundary may be subdivided into a unique set of corners (points) and edges (open line segments), called the features of . Let denote this feature set. Our representation of ensures this local property of : for any point , if is the closest feature to , then we can decide if is inside or not. To see this, first note that if is a corner, then is outside iff is a convex corner of . But if is an edge, our representation assigns an orientation to such that is inside iff lies to the left of the oriented line through .

3 Star-Shaped Robots

We first consider star-shaped robots. A star-shaped region is one for which there exists a point such that any line through intersects in a single line segment. We call a center of . Note that is not unique. When a robot is a star-shaped polygon, we decompose into a set of triangles that share a common vertex at a center . The rotations of the robot about the point can then be reduced to the rotations of “nice” triangles about . The soft predicates of nice triangles will be easy to implement because their footprints have special representations.

3.1 Nice Shapes for Rotation

(a) triangular set(unbounded case)apex(c) swept area by a nice triangle(b) truncated triangular set (d) sweeping to
Figure 3: Truncated triangular set and swept areas.

From now on, by a triangular set we mean a subset which is written as the non-redundant intersection of three closed half-spaces: . Non-redundant means that we cannot express as the intersection of only two half-spaces. Note that if is bounded, this is our familiar notion of a triangle with 3 vertices. But might be unbounded and have only 2 vertices as in generalized-tri-b(a). If is a triangular set, we may arbitrarily call one of its vertices the apex and call the resulting a pointed triangular set. By a truncated triangular set (TTS), we mean the intersection of a pointed triangular set with any disc centered at its apex , as shown in generalized-tri-b(b).

Notation for Angular Range: It is usual to identify (unit circle) with the interval where and are identified. Let . Then denote the range of angles from counter-clockwise to . Thus and are complementary ranges in . If , then its width, is defined as if , and otherwise. Moreover, we will write “” to mean that .

Fix an arbitrary bounded triangular set , represented by its three vertices where is the apex. For , let denote the footprint of after rotating counter-clockwise (CCW) by about the apex. If , we write . The sets and are called footprints of at and , respectively. If , write for , and call the swept area as rotates from to .

One of our concerns is to ensure that the swept area is “nice.” Consider an example where is a triangular set with apex (see generalized-tri-b(c)). Consider the area swept by rotating in a CCW direction about its apex to position . This sweeps out the truncated triangular set shown in generalized-tri-b(b). This truncated triangular set (TTS) is desirable since it can be easily specified by the intersection of three half-spaces and a disc. On the other hand, if is the triangular set in generalized-tri-b(d), then no rotation of would sweep out a truncated triangular set. So the triangular set in generalized-tri-b(d) is “not nice,” unlike the triangular set in generalized-tri-b(c).

In general, let be a bounded triangular set. Let denote the corresponding angles at . We say is nice if either or is at least (). We call the corresponding vertex ( or ) a nice vertex. Assuming is non-degenerate and nice, there is a unique nice vertex. In the following, we assume (w.l.o.g.) that is the nice vertex. The reason for defining niceness is the following.

Let be a pointed triangular set. Then is nice iff for all (), the footprints and are truncated triangular sets (TTS).

Let be a star-shaped polygonal region with as center. If the boundary of is an -gon, then we can decompose into an essentially disjoint222 A set where each is said to be essentially disjoint if the interiors of the ’s are pairwise disjoint. union of at most bounded triangular sets (i.e., at most triangles) that are nice and have as the apex.

3.2 Complex Predicates and T/R Subdivision Scheme

For complex robots in general (not necessarily star-shaped), we can exploit the remarkable decomposability property of soft predicates. More specifically, suppose where each is a triangle or other shapes and not necessarily pairwise disjoint. If we have soft predicates for each (where is a box), then we immediately obtain a soft predicate for defined as follows:

(1)

Let and be the soft version of an exact predicate . Recall wang-chiang-yap:motion-planning:15 ; sss2 that is -effective if for all boxes , if then .

Proposition A.  
(1) is a soft version of the exact classification predicate for .  
(2) Moreover, if each is -effective, then is -effective.

We need -effectivity in soft predicates in order to ensure resolution-exactness; see wang-chiang-yap:motion-planning:15 ; sss2 where this proposition was proved. The following is a soft predicate for :

Moreover, if each is -effective, then then is also -effective.  
There are two important remarks. First, this proposition is false if the and were exact predicates. More precisely, suppose is the exact predicate for and is the exact predicate for each . It is true that if then for all . But if , it does not follow that for some . Second, the predicates for all the ’s must be based on a common coordinate system. As mentioned in Sec. 1, the soft predicate construction for a triangle robot in wang-chiang-yap:motion-planning:15 does not work here. A technical contribution of this paper is the design of soft predicates for all the ’s that are based on a common coordinate system. In the case of star-shaped robots, we apply Lemma 3.1 and use the apex as the origin of this common coordinate system. Let be the length of the longer edge out of in . We define as (i.e., is the radius of the circumcircle of centered at ).

T/R Splitting. The simplest splitting strategy is to split a box into congruent subboxes. In the worst case, to reduce all boxes to size requires time ; this complexity would not be practical for . In luo-chiang-lien-yap:link:14 ; yap-luo-hsu:thicklink:16 we introduced an effective solution called T/R splitting which can be adapted to configuration space333 The configuration space of planar rigid robots is where is the unit circle representing angles . in the current paper. Write a box as a pair where is the translational box and an angular range . We say box is -small if and are both -small; the former means the width of is ; the latter means the angle (in radians) satisfies . Our splitting strategy is to only split (leaving ) as long as is not -small. This is called a T-split, and produces 4 children. Once is -small, we do binary splits of (called R-split) until is -small. We discard when it is -small. The following lemma (and proof) in wang-chiang-yap:motion-planning:15 can be carried over here:

(wang-chiang-yap:motion-planning:15 ) Assume . If is -small and is a square, then the Hausdorff distance between the footprints of at any two configurations in is at most .

Soft Predicates. Suppose we want to compute a soft predicate to classify boxes . Following the previous work wang-chiang-yap:motion-planning:15 ; luo-chiang-lien-yap:link:14 , we reduce this to computing a feature set . The feature set of is defined as comprising those features such that sepmb (m_B,f)≤r_B + r_0 where and are respectively the midpoint and radius of the translational box of (also call them the midpoint and radius of ), and
denotes the separation of two Euclidean sets . We say that is empty if is empty but is not, where is the parent of . We may assume the root is never empty. If is empty, it is easy to decide whether is  or : since the feature set is non-empty, we can find the such that is minimized. Then , and by the local property of features (see Feature-Based Approach in Sec. 2), we can decide if is inside ( is ) or outside ( is ).

For a box where , we maintain its feature set as above. But when , we compute its feature set as follows. Recall that we decompose into a set of nice triangles with a common apex . For each , consider the footprint of with at and rotating about from to , where . By Lemma 3.1 the resulting swept area is a truncated triangular set (TTS); call it . We define (cf. wang-chiang-yap:motion-planning:15 ) A shape is be any closed simply-connected (possibly unbounded) subset of . For , for a 2D shape the -expansion of , denoted by , to be the Minkowski sum of with the of radius centered at the origin. For a TTS, recall that where is an unbounded triangular set (with each a half space) and is a disk (generalized-tri-b). Note that is a proper subset of ; a theorem in the next section gives an exact representation of . We now specify the feature set : for each , let comprise those features satisfying (replacing with in Eq. (LABEL:eq:sepmb)), such that also intersects the -expansion of . We can think of as a collection of these ’s, each of which is used by the soft predicate so that we can apply Proposition A.

4 General Complex Robots

When is a general polygon, not necessarily star-shaped, we can still decompose into a set of triangles (), and consider the rotation of these triangles relative to a fixed point (we may identify with the origin). In this section, we define what it means for to be “nice” relative to a point . If lies in the interior of , we could decompose into at most nice pointed triangles at , as in the previous section. Henceforth, assume that does not lie in the interior of .

4.1 Basic Representation of Nicely Swept Sets

Let be any non-degenerate triangular region defined by the vertices . Let the origin be outside the interior of . We define what it means for to be “nice relative to .” W.l.o.g., let where is the Euclidean norm.

We say that is nice if the following three conditions hold: nice A,B-A≥0,  A,C-A≥0,  B,C-B≥0. Here

denotes the dot product of vectors

.

A more geometric view of niceness is as follows (see nice-tri-general). Draw three concentric circles centered at with radii , respectively. Two circles would coincide if their radii are equal, but we will see that the distinctness of the vertices and niceness prevent such coincidences. Let be the line tangent to the circle of radius and passing through the point . Let denote the closed half-space bounded by and not containing . The first condition in nice says that . Similarly, the second condition says that . Finally, the last condition says that (where is analogous to ).

Figure 4: Nice triangle .
Figure 5: Nicely swept set (NSS, in blue) with in CCW order.
Figure 6: Expansion of of Fig. 6 (in red).
Figure 7: Nicely swept set (NSS, in blue) with in CW order.

If is a nice triangle, then is called a nicely swept set (NSS). See  nicely-swept-set, where is shaded in blue. Let be the triangle and be . W.l.o.g., assume444 In case appear in clockwise (CW) order, the boundary of can be similarly decomposed into two parts, comprising the swept segment and the triangle . See nicely-swept-set-flip. that appear in counter-clockwise (CCW) order as indicated in nicely-swept-set. Then we can subdivide into two parts: the triangle and another part which we call a swept segment.

Notation for Swept Segment: if is the line segment , then write for this swept segment. The boundary of is decomposed into the following sequence of four curves given in clockwise (CW) order: (i) the arc centered at of radius from to , (ii) the segment , (iii) the arc centered at of radius from to , (iv) the segment .

Our next goal is to consider -expansion of the swept segment, i.e., oplus X = S[α,β]⊕Disc(s). Specifically, we want an easy way to detect the intersection between this expansion with any given feature (corner or edge). To do so, we want to express as the union of “basic shapes.” A subset of is a -basic shape if it is a half-space, a disc or complement of a disc. We write for the disc of radius centered at , and for the annulus with inner radius and outer radius centered at . A shape is said to be -basic if it can be written as the finite intersection where ’s are -basic shapes. The -size of is the minimum in such an intersection. So polygons with sides have -size of . Truncated triangular sets have -size of . We need some other -basic shapes:

Strips: is the region between the two parallel lines and . Here are distinct points.

Truncated strips: is the intersection of with an annulus; the boundary of this shape is comprised of two line segments and and two arcs and from the boundary of the annulus.

Sectors: denotes any region bounded by a circular arc and two segments and .

Finally, a shape is said to be -basic if it can be written as a finite union of -basic shapes, where ’s are -basic. We call a basic representation of . The -size of the representation is the sum of the -sizes of ’s. Thus, for any box , the -expansion of is a -basic shape since it is the union of four discs and an octagon. We now consider the case where is the -expansion of a swept segment . We first decompose into two shapes as follows: suppose lies on the circle of radius . Considering both cases of being in CCW and CW orders, there are two possible representations:
(1) If is parallel to and , then we have decomp1 S[α,β]= Sector(A’,C’,C”) ∪TruncStrip(A,C;A’,C”).
(2) If is parallel to and , then we have decomp2 S[α,β]= Sector(A,C,C”) ∪TruncStrip(A,C”;A’,C’). The swept segment in nicely-swept-set supports the representation decomp1 but not decomp2, while the swept segment in nicely-swept-set-flip supports the representation decomp2 but not decomp1. Note that they are symmetric cases, with in CCW order in nicely-swept-set and in CW order in nicely-swept-set-flip. Also, if the angular range of is greater than degrees and the points are collinear, then both representations fail! We next show when at least one of the representations succeeds:

Assume the width of the angular range is at most . Then swept segment can be decomposed into a sector and a truncated strip as in decomp1 or decomp2.

Clearly, the -expansion of a sector is -basic. This is also true for truncated strips (w.l.o.g., considering that in the representation decomp1):

Let . There is a basic representation of of the form where ’s are discs and is the intersection of a convex hexagon with an annulus.

Combining all these lemmas, we conclude:

Let be a nicely swept set where has width . Then can be decomposed into a triangle, a sector and a truncated strip. The -expansion of has a basic representation which is the union of the -expansions of the triangle, sector and truncated strip.

The complexity of testing intersection of -basic shapes with any feature is proportional to its -size, which is . This theorem assures us that the constants in “” is small. Note that it is not correct to test if a line segment intersects a -basic shape by just testing if intersects every , since could intersect every but not all in the same place(s) so that . Therefore, we need to maintain the common intersections between and all ’s tested so far as we loop over all ’s; at the end, intersects if and only if there is at least one non-empty set of common intersections. Since the complement of a disk is non-convex, in general this process could result in many sets/segments of common intersections to maintain. Fortunately, there is at most one complement of a disk in our decomposition of an . Thus it is enough to maintain just a single set/segment of the common intersection of with all other -basic shapes, and check with the complement of a disk only at the end.

Consider the triangular set , where the notation refers to the half space to the left of the line passing through and , with orientation from to . Also, let denote the disc of radius centered at . Then the nicely swept set (NSS) just defined (see nicely-swept-set) is exactly .

4.2 Partitioning an -gon into Nice Triangles

Suppose is an -gon. We can partition it into triangles. W.l.o.g., there is at most one triangle that contains the origin . We can split that triangle into at most 6 nice triangles, using our technique for star-shaped polygons (Lemma 3.1).

If is an arbitrary triangle and is exterior to , then we can partition into at most nice triangles.

The number in this lemma is the best possible: if is a triangle with circumcenter , then any partition of into nice triangles would have at least triangles because we need to introduce vertices in the middle of each side of .

Let be an -gon.
(i) Given any triangulation of into triangles, we can refine the triangulation into a triangulation with nice triangles.
(ii) This bound is tight in this sense: for every , there is a triangulation of whose refinement has size .  

4.3 Soft Predicates and T/R Subdivision Scheme

We can now follow the same paradigm as for star-shaped robots in Sec. 3.2. We first apply Theorem 4.2(i) to partition the robot into a set of nice triangles, , where all ’s share a common origin , and we will use the soft predicates developed for and apply Proposition A. The origin plays a similar role as the apex in Sec. 3.2. The T/R splitting scheme is exactly the same: we first perform T-splits, splitting only the translational boxes until they are -small, and then we perform R-splits, splitting only the rotational boxes until they are -small. Essentially the top part of the subdivision tree is a quad-tree, and the bottom parts are binary subtrees (see Sec. 3.2).

The feature set for a subdivision box where we perform T-splits is the same as before; the only difference is that now for a box where we perform R-splits, we use a new feature set for each nice triangle where is not at its vertex (there are at most 6 nice triangles with at a vertex/apex; see Theorem 4.2(i)). Suppose with . Let . Also, suppose the angle range of box is . Recall the footprint of is a nicely swept set (NSS); denote it . Then the new feature set for comprises those where and also intersects the -expansion of (where and are the midpoint and radius of ).

5 Experimental Results

Robot (# sides) (# triangles)
L-shaped 6 4
snowflake 18 24
S-shaped 12 26
3-legged 14 20
C-shaped 18 22
Table 1: Robot Statistics.
Exp# Robot Envir. R P? Time
0 L-shaped gateway 50 2 (18, 98, 340) (458,119,270) Yes 10.106
1 L-shaped gateway 50 4 (18, 98, 340) (458,119,270) No 8.431
2 snowflake sparks 56 2 (108, 136, 0) (358, 155, 0) Yes 17.846
3 snowflake sparks 56 2 (108, 136, 0) (358, 155, 180) Yes 3.370
4 S-shaped sparks 74 4 (132, 80, 90) (333, 205, 90) Yes 34.284
5 S-shaped sparks 74 4 (132, 80, 90) (333, 205, 60) No 57.371
6 3-legged sparks 70 2 (108, 136, 0) (368, 155, 0) Yes 41.745
7 L-shaped corridor 68 2 (75, 420, 0) (370, 420, 0) Yes 4.012
8 L-shaped corridor 68 3 (75, 420, 0) (370, 420, 0) Yes 1.926
9 L-shaped corridor 68 5 (75, 420, 0) (370, 420, 0) Yes 2.684
10 L-shaped corridor-L 68 5 (75, 420, 0) (370, 420, 0) No 2.908
11 L-shaped corridor-L 68 3 (75, 420, 0) (370, 420, 0) Yes 2.255
12 C-shaped corridor-S 80 4 (80, 450, 0) (380, 450, 0) Yes 26.200
13 S-shaped maze 38 2 (38, 38, 0) (474, 474, 90) No 90.097
14 S-shaped* maze 38 2 (38, 38, 0) (474, 474, 90) Yes 79.518
Table 2: Running Our Planner (R: radius of the robot’s circumcircle around its rotation center; P?: path found? (Yes/No); Time is in s; S-shaped*: thin version).
# Time/P? PRM RRT EST KPIECE
0 10.106/Y 4.18/2.53/1 42.13/38.49/1 76.22/110.44/0.9 300/0/0
2 17.846/Y 9.22/6.82/1 210.41/144.25/0.3 271.75/89.31/0.1 240.00/126.47/0.2
3 3.370/Y 300/0/0 300/0/0 300/0/0 300/0/0
4 34.284/Y 5.93/7.20/1 217.33/134.53/0.3 300/0/0 300/0/0
5 57.371/N 300/0/0 300/0/0 300/0/0 300/0/0
6 41.745/Y 2.72/4.89/1 154.22/141.77/0.5 104.32/78.10/0.7 3.16/4.28/1
8 1.926/Y 0.63/0.55/1 300/0/0 3.02/4.71/1 0.41/0.28/1
11 2.255/Y 1.49/0.84/1 300/0/0 241.24/124.88/0.2 1.58/1.47/1
12 26.200/Y 3.16/4.21/1 300/0/0 172.506/120.38/0.7 93.88/88.03/0.8
13 90.097/N 300/0/0 300/0/0 300/0/0 300/0/0
14 79.518/Y 300/0/0 236.72/106.44/0.3 300/0/0 39.81/91.57/0.9
Table 3:

Comparing with OMPL (“#”: Exp#; “Time/P?”: our run time (in s)/path found? (Y/N). Each OMPL method: Average Time (in s)/Standard Deviation/Success Rate, over 10 runs).

envir-60.07186Six Environments in our experiments. We have implemented our approaches in C/C++ with Qt GUI platform. The software and data sets are freely available from the web site for our open-source  core-download-link . All experiments are reproducible as targets of Makefiles in . Our experiments are on a PC with one 3.4GHz Intel Quad Core i7-2600 CPU, 16GB RAM, nVidia GeForce GTX 570 graphics and Linux Ubuntu 16.04 OS. The results are summarized in exp-result and exp-ompl. exp-result is concerned only with the behavior of our complex robots; exp-ompl gives comparisons with the open-source OMPL library OMPL . The robots are as shown in robot; their statistics are given in rob-stats.

There are two sets of experiments, for two complex robots: an -shape robot and the snow flake robot in robot(d). For each robot, we run it in one of the four environments in envir-new or in the parking environment in gui. In exp-result, radius is a robot parameter. More specifically, it is the distance from the origin to the furthest vertex of a robot. The resolution exactness parameter is , and and are the start and goal configurations. The column #Box gives the total number of configuration boxes that are created in the subdivision search to find a path (or to conclude NO-PATH).

We select some interesting experiments to analyze characteristic behavior of our planner. Please see exp-result and the video ( https://cs.nyu.edu/exact/gallery/complex/complex-robot-demo.mp4). In Exp0-1, we show how the parameter affects the result. With a narrow gateway, when we change from to , the output changes from a path to NO-PATH for the same configuration. In Exp2-3, we observe how the snowflake robot rotates and maneuvers to get from the start to two different goals. For Exp4-5, the difference is in the angles of the goal configuration; in Exp5 this is designed to be an isolated configuration and the planner outputs NO-PATH  as desired. Exp6 shows how the robot squeezes among the obstacles to move its complex shape through the environment. Exp7-9 use the same L-shaped robot, configurations and the environment; only varies. The planner can find three totally different paths. When is small (Exp7), the path is very carefully adjusted to move the robot around the obstacles. When is larger (Exp8), the planner finds an upper path with a higher clearance. When is even larger (Exp9), the planner chooses a very safe but much longer path at the bottom. Note that using a larger usually makes the search faster, since we stop splitting boxes smaller than , but a longer path can make the search slower. In Exp10-11, we modify the environment of Exp7-9 by putting a large obstacle at the bottom, which forces the robot to find a path at the top. Exp12 uses an environment similar to those in Exp7-11 but with much smaller scattered obstacles. It is designed for the C-shaped robot, which can rotate while having an obstacle in its pocket. Exp13-14 use a challenging environment where the small scattered obstacles force the S-shaped robot to rotate around and only the “thin” version (Exp14, also in Fig. 3 “maze”) can squeeze through.

In Table 3 we compare our planner with several sampling algorithms in OMPL: PRM, RRT, EST, and KPIECE. These experiments are correlated to those in exp-result (see the Exp #). Each OMPL planner is run 10 times with a time limit 300 seconds (default), where all planner-specific parameters use the OMPL default values. We see that for OMPL planners there are often unsuccessful runs and they have to time out even when there is a path. On the other hand, our algorithm consistently solves the problems in a reasonable amount of time, often much faster than the OMPL planners, in addition to being able to report NO-PATH.

6 Conclusions

Although the study of rigorous algorithms for motion planning has been around for over 40 years, there has always been a gap between such theoretical algorithms and the practical methods. Our introduction of resolution-exactness and soft predicates on the theoretical front, together with matching implementations, closes this gap. Moreover, it eliminated the “narrow passage” problem that plagued the sampling approaches. The present paper extends our approach to challenging planning problems for which no exact algorithms exist.

What are the current limitations of our work? We implement everything in machine precision (the practice in this field). But it can be easily modified to achieve the theoretical guarantees of resolution-exactness if we use arbitrary precision BigFloats number types.

We pose two open problems: One is to find an optimal decomposition of -gons into nice triangles (currently, we simply give an upper bound). Such decompositions will have impact for practical complex robots. Second, we would like to develop similar decomposability of soft predicates for complex rigid robots in .

References

  • [1] F. Avnaim, J.-D. Boissonnat, and B. Faverjon. A practical exact motion planning algorithm for polygonal objects amidst polygonal obstacles. In B. J.D. and L. J.P., editors, Geometry and Robotics, LNCS Vol 391. Springer, Berlin-Heidelberg, 1989.
  • [2] M. Barbehenn and S. Hutchinson. Efficient search and hierarchical motion planning by dynamically maintaining single-source shortest paths trees. IEEE Trans. Robotics and Automation, 11(2), 1995.
  • [3] J. Basch, L. Guibas, D. Hsu, and A. Nguyen. Disconnection proofs for motion planning. In IEEE Int’l Conf. on Robotics Animation, pages 1765–1772, 2001.
  • [4] H. Bennett, E. Papadopoulou, and C. Yap. Planar minimization diagrams via subdivision with applications to anisotropic Voronoi diagrams. Eurographics Symposium on Geometric Processing, 35(5), 2016. SGP 2016, Berlin, Germany. June 20-24, 2016.
  • [5] H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L. E. Kavraki, and S. Thrun. Principles of Robot Motion: Theory, Algorithms, and Implementations. MIT Press, Boston, 2005.
  • [6] Core Library. https://cs.nyu.edu/exact/core_pages/downloads.html.
  • [7] D. Halperin, E. Fogel, and R. Wein. CGAL Arrangements and Their Applications. Springer-Verlag, Berlin and Heidelberg, 2012.
  • [8] C.-H. Hsu, Y.-J. Chiang, and C. Yap. Rods and rings: Soft subdivision planner for R^3 x S^2. In Proc. 35th Int’l Symp. on Comp. Geom.(SoCG 2019), June 18-21, 2019. To appear. CG Week 2019, Portland Oregon. Also in arXiv:1903.09416.
  • [9] J.-C. Latombe. Robot Motion Planning. Kluwer Academic Publishers, 1991.
  • [10] S. M. LaValle. Planning Algorithms. Cambridge University Press, Cambridge, 2006.
  • [11] Z. Luo, Y.-J. Chiang, J.-M. Lien, and C. Yap. Resolution exact algorithms for link robots. In Proc. 11th Intl. Workshop on Algorithmic Foundations of Robotics (WAFR ’14), volume 107 of Springer Tracts in Advanced Robotics (STAR), pages 353–370, 2015. Aug. 3-5, 2014, Boǧazici University, Istanbul, Turkey.
  • [12] V. Milenkovic, E. Sacks, and S. Trac. Robust complete path planning in the plane. In Proc. 10th Workshop on Algorithmic Foundations of Robotics (WAFR 2012), Springer Tracts in Advanced Robotics, vol.86, pages 37–52. Springer, 2012.
  • [13] O. Salzman, M. Hemmer, B. Raveh, and D. Halperin. Motion planning via manifold samples. In Proc. European Symp. Algorithms (ESA), pages 493–505, 2011.
  • [14] I. Şucan, M. Moll, and L. Kavraki. The Open Motion Planning Library. IEEE Robotics & Automation Magazine, 19(4):72–82, 2012. http://ompl.kavrakilab.org.
  • [15] C. Wang, Y.-J. Chiang, and C. Yap. On soft predicates in subdivision motion planning. Comput. Geometry: Theory and Appl. (Special Issue for SoCG’13), 48(8):589–605, Sept. 2015.
  • [16] C. Yap. Soft subdivision search in motion planning. In A. Aladren et al., editor, Proc. 1st Workshop on Robotics Challenge and Vision (RCV 2013), 2013. Robotics Science and Systems Conf. (RSS 2013), Berlin. In arXiv:1402.3213. Full paper: http://cs.nyu.edu/exact/papers/.
  • [17] C. Yap. Soft subdivision search and motion planning, II: Axiomatics. In Frontiers in Algorithmics, volume 9130 of Lecture Notes in Comp. Sci., pages 7–22. Springer, 2015. Plenary talk at 9th FAW. Guilin, China. Aug. 3-5, 2015.
  • [18] C. Yap, Z. Luo, and C.-H. Hsu. Resolution-exact planner for thick non-crossing 2-link robots. In Proc. 12th Intl. Workshop on Algorithmic Foundations of Robotics (WAFR ’16), 2016. Dec. 13-16, 2016, San Francisco. The appendix in the full paper (and arXiv from http://cs.nyu.edu/exact/ (and arXiv:1704.05123 [cs.CG]) contains proofs and additional experimental data.
  • [19] L. Zhang, Y. J. Kim, and D. Manocha. Efficient cell labeling and path non-existence computation using C-obstacle query. Int’l. J. Robotics Research, 27(11–12):1246–1257, 2008.
  • [20] B. Zhou, Y.-J. Chiang, and C. Yap. Soft subdivision motion planning for complex planar robots. In Proc. 26th European Symposium on Algorithms (ESA 2018), pages 73:1–73:14, 2018. Helsinki, Finland, Aug. 20-24, 2018.
  • [21] D. Zhu and J.-C. Latombe. New heuristic algorithms for efficient hierarchical path planning. IEEE Transactions on Robotics and Automation, 7:9–20, 1991.

APPENDIX: Proofs

Lemma 1. Let be a pointed triangular set. Then is nice iff for all (), the footprints and are truncated triangular sets (TTS).

If is nice, and are truncated triangular sets (TTS); this is easily seen in generalized-tri-b(c).

Conversely, if is not nice, let us assume that (e.g., generalized-tri-b(d)). We claim that for sufficiently small , either or is not a TTS. Assume (w.l.o.g.) that are in CCW order; we show that is not a TTS.

If is not nice, then . Let intersects the (the circle centered at that passes through ) at . Let , since . Note that a TTS is a convex set as it is the intersection of three half-spaces and one disc; all of them are convex and thus the intersection is also convex. However, for any , is not a TTS since will intersect inside (see not-nice-tri-set) — this makes non-convex and thus it is not a TTS.

\begin{picture}(11249.0,5795.0)(676.0,-6006.0)\end{picture}
Figure 8: Proof of Lemma 3.1: is not a truncated triangular set (TTS).

Lemma 2. Let be a star-shaped polygonal region with as center. If the boundary of is an -gon, then we can decompose into an essentially disjoint union of at most bounded triangular sets (i.e., at most triangles) that are nice and have as the apex.

First, for each vertex of we add a segment connecting and . This decomposes into a disjoint union of triangles (since is star-shaped). Now consider each of the resulting triangle and let be the apex of . If is not nice, then both angles and (corresponding to vertices and ) are less than , and we can add a segment that is perpendicular to edge and intersects at where is in the interior of . This effectively decomposes into two nice triangles and with being the common apex. In this way, we can decompose into at most nice triangles that have as the apex.

Lemma 4. Assume the width of the angular range is at most . Then swept segment can be decomposed into a sector and a truncated strip as in decomp1 or decomp2

Our goal is to choose the point so that either decomp1 or decomp2 holds. Let the swept segment be , with and . Let (resp., ) be the point such that (resp., ) and (resp., ) are collinear. Then , bounded by the arc centered at , contains either or . If it contains (see nicely-swept-set), then we choose such that and is parallel to , and thus decomp1 holds. By symmetry, if the sector contains (see nicely-swept-set-flip), we can choose so that decomp2 holds.

Lemma 5. Let . There is a basic representation of of the form where ’s are discs and is the intersection of a convex hexagon with an annulus.

See nicely-swept-set for a figure of . Let where denote the disc with center of radius . These discs are outlined in green in Minkowski-sum. The boundary of each () intersects the boundary of in a circular arc where is closer to than . Let be the half space containing and bounded by the line through . We need to check that these half spaces do indeed contain . Also, let (resp., ) be the half space containing and bounded by the line through and (resp., and ). Note that and are parallel. Then we see that is a convex hexagon containing , and the intersection is outlined in red in Minkowski-sum. Observe that this intersection covers all of .

This construction is valid as long as , i.e., the annulus is a true annulus. When , the boundary of no longer has an inner arc of radius , but degenerates into a concave vertex where the two circles of radius centered at and (resp.) meet.

Theorem 6. Let be a nicely swept set where has width . Then can be decomposed into a triangle, a sector and a truncated strip. The -expansion of has a basic representation which is the union of the -expansions of the triangle, sector and truncated strip.

We know that can be decomposed into a triangle and a swept segment. The swept segment, since has width , can be further decomposed into a sector and a truncated strip. The expansions of the triangle and sector are clear; the expansion of the truncated strip was the subject of the previous lemma.

Lemma 7. If is an arbitrary triangle and is exterior to , then we can partition into at most nice triangles.

Let . In the worst case, all three niceness conditions for (i.e., , and , where ; recall the geometric view of niceness described right after Eq. nice) are violated. W.l.o.g., suppose that among the three edges of , is the closest to . Let be the point on such that , and similarly for and ; see 4-nice-triangles. Then we add segments to decompose into 4 triangles and . Note that the line tangent to the circle of radius (centered at ) and passing through the point coincides with ; similarly, the line coincides with and coincides with . As before, is the half space bounded by and not containing ; similarly for and . For the triangle , note that since is closer to than (so ), (so ) and (so ). Thus the three niceness conditions for the triangle are: , and . Again, these three conditions are satisfied due to the facts that is closer to than , and , i.e., these conditions are automatically satisfied due to the construction of and . Similarly, the three niceness conditions for the triangle are: , and , which are again satisfied due to the construction of and . Symmetrically, the triangles and are both nice due to the construction of and . Therefore can be decomposed into at most 4 nice triangles.

We refer to 4-nice-triangles. Note that segments and are perpendicular to segments and , respectively. It can be easily verified that each of the 4 triangles and is a nice triangle.

\begin{picture}(6015.0,3510.0)(976.0,-3496.0)\end{picture}
Figure 9: Proof of Lemma 4.2: A triangle with the origin in the exterior can be decomposed into at most 4 nice triangles.

Theorem 8. Let be an -gon.
(i) Given any triangulation of into triangles, we can refine the triangulation into a triangulation with nice triangles.
(ii) This bound is tight in this sense: for every , there is a triangulation of whose refinement has size .

(i) In the given triangulation of , we might have a triangle containing . This triangle can be triangulated into at most nice triangles (Lemma 3.1). By Lemma 4.2, the remaining triangles can be refined into nice triangles. The final count is .

(ii) We construct an -gon whose vertices are all on the unit circle. Note that all such vertices are of the form . For the first triangle , pick the vertices . Call these vertices . Choose the origin inside so that for each triangle we have both and . Therefore each triangle must be split into nice triangles and overall must be split into nice triangles. If , our result is verified. If , we must add additional vertices. Define the vertex , (note that and