Motion Planning for a Pair of Tethered Robots
MMotion Planning for a Pair of Tethered Robots
Reza H. Teshnizi · Dylan A. ShellAbstract
Considering an environment containing polyg-onal obstacles, we address the problem of planning mo-tions for a pair of planar robots connected to one an-other via a cable of limited length. Much like prior prob-lems with a single robot connected via a cable to a fixedbase, straight line-of-sight visibility plays an importantrole. The present paper shows how the reduced visibil-ity graph provides a natural discretization and capturesthe essential topological considerations very effectivelyfor the two robot case as well. Unlike the single robotcase, however, the bounded cable length introduces con-siderations around coordination (or equivalently, whenviewed from the point of view of a centralized planner,relative timing) that complicates the matter. Indeed,the paper has to introduce a rather more involved for-malization than prior single-robot work in order to es-tablish the core theoretical result—a theorem permit-ting the problem to be cast as one of finding pathsrather than trajectories. Once affirmed, the planningproblem reduces to a straightforward graph search withan elegant representation of the connecting cable, de-manding only a few extra ancillary checks that ensuresufficiency of cable to guarantee feasibility of the so-lution. We describe our implementation of A (cid:63) search,and report experimental results. Lastly, we prescribean optimal execution for the solutions provided by thealgorithm.
Keywords
Motion Planning · Tethered Robots · Multi-Robot Coordination · A* Search
R.H. Teshnizi · D.A. ShellDistributed AI and Robotics Laboratory,Department of Computer Science and Engineering,Texas A&M University, College Station, TX 77843, USA.E-mail: [email protected]: [email protected]
Fig. 1: Rope team at a ridge nearing a summit. (Source:Wikimedia Commons)
In recent years, a variety of techniques have been devel-oped to plan motions for a tethered mobile robot (Igarashiand Stilman, 2010; Teshnizi and Shell, 2014; Kim et al.,2014). A tether can be useful as a conduit for poweror communication but the main motivating applicationfor robotic tethers is in navigation of rovers in extremeterrain, where the tether can help provide physical se-curity. Examples of robotic rovers equipped in this wayinclude TRESSA (Huntsberger et al., 2007), Axel andDuAxel (Nesnas et al., 2012), vScout (Stenning et al.,2015), and TReX (McGarey et al., 2018), among oth-ers. Humans deal with extreme terrain too. A commonpractice among mountaineers, as a measure of protec-tion against falling, is to form a group that can movetogether while the members are roped to one another.This forms what is referred to as a rope team (Good- a r X i v : . [ c s . R O ] F e b Reza H. Teshnizi, Dylan A. Shell ing and Martin, 2014) (Fig. 1). From the perspectiveof motion planning, one might interpret a rope teamas a practical scenario in which a tethered robot’s baseis itself subject to motion. A prominent robotic exam-ple of comparable operation is DuAxel, where two Axelrovers are connected to a central module. DuAxel is de-signed to work as a mother-daughter ship , having oneAxel remain stationary with the central module whilethe other explores the terrain. Indeed, enabling bothrovers to be deployed simultaneously may benefit bothagents and improve the versatility of the design.One key to efficient solution of planning problemsin finding a suitable representation, ideally one that ex-presses constraints and is amenable to adaptation andgeneralization to various requirements. Much like ourearlier work (Teshnizi and Shell, 2014), we take ad-vantage of the properties of reduced visibility graph(Latombe, 1991). Namely, we show certain characteris-tics of straight line motions enable us to find solutionsto tethered pair problem with minimal book keeping.Unlike that work, however, the proofs proceed withoutneeding to supply geometric or topological insight fromthe structure of configuration space (c-space). We dodiscuss, in Section 8, some of our understanding of thec-space of this problem.The theory established in this work shows that weshall not be concerned with the intermediate state ofthe cable: as long as we can achieve a goal configurationthat is permitted by the length of the available cable,one can provide a planner to execute the motions thatwill transform the initial cable configuration to its finalconfiguration. This would be trivial if the constraintsinvolved were purely topological or we considered onlya single robot. But with a finite cable, what one robotuses is unavailable to the other, coupling their motionsin space and time. On the basis of this theory, we in-troduce a tree data structure that represent differentcable configurations up to homotopy. Each branch inthe tree down to a certain node is a representation ofthe shortest path required for the tethered pair to ar-rive at that node’s cable configuration. We have imple-mented A (cid:63) search to expand the search tree and findan optimal solution while keeping track of the cable’sconfiguration up to homotopy. Lastly, we detail how toturn paths produced by the planner into trajectories forthe motions that also minimize a time cost.
We are interested in what is perhaps the most naturalmotion planning question for a tethered pair of robots,namely finding paths to take a pair of tethered robots from some initial configuration to a goal one, never vi-olating a bound on the tether’s length throughout themotion. Although motion planning for a single tetheredrobot has been extensively studied (Shnaps and Rimon,2013; Teshnizi and Shell, 2014; Kim et al., 2014; Tesh-nizi and Shell, 2016a; McCammon and Hollinger, 2017;McGarey et al., 2017), the literature reports compara-tively little work on motion planning problems involv-ing pairs of robots tethered to one another.A notable exception is that pairs of conjoined robotshave been studied for purposes of object manipulation.Kim et al. (2013) studied object separation using apair of robots connected by a cable. Though superfi-cially similar to our problem, as it involves the motionof a mutually connected pair of robots, the separationproblem imposes quite a different set of constraints toa shortest path planning problem. For object separa-tion the solution is only required to satisfy a homo-topy requirement, allowing the robots to choose anyarbitrary goal in the workspace that can satisfy suchconstraint. Consequently, in that work, the two robotsmove to the boundaries of the workspace. (This assump-tion also helps distinguish between separating versusnon-separating configurations elegantly and concisely.)As Kim et al. are addressing a problem where the goal isspecified topologically and they are not concerned witha cable of finite length, several of the complications wetackle do not arise in their setting.More recently, Kim and Shell (2018) demonstrateda physical multi-robot system in which robots can makeor break tether connections dynamically; a planner ex-ploits this capability to find ways in which a robot teamcan manipulate objects efficiently, either with singlerobots operating concurrently, or as coupled pairs, ascalled for by the particular problem instance. A partof that problem is combinatorial and the work uses asampling-based method, the present work being distin-guished from that work on both fronts.Our own prior work on finding short paths for apair of tethered robots (Teshnizi and Shell, 2016b), at-tempted to use the solution to a single robot problemas an algorithmic building block. That approach wasdevised primarily as a means to build intuition for thetopology of the four dimensional configuration space;the algorithm outlined in that work is inadequate, be-ing neither a solution to the complete problem nor onethat always yields optimal paths. Toward the end of thepaper, we will return to this matter of visualizing thec-space in light of the correct and complete algorithmthat is in this paper. otion Planning for a Pair of Tethered Robots 3
Let O = { o , o , o , . . . , o n } be a (possibly empty) setof pairwise disjoint polygonal obstacles with vertices(written verts( O )) in R , with boundary curves δo , δo ,. . . , δo n , respectively. We will write W for the free space,so W = ( R \ (cid:83) ni =1 o i ) ∪ ( (cid:83) ni =1 δo i ). Further, let robots a and b be two unoriented points in W that are con-nected to one another via a cable of finite length. Forconciseness, let I = [0 ,
1] be the unit interval.
Definition 1 ( trpmpp ) The tethered robot pair mo-tion planning problem ( trpmpp ) is a tuple,( W , O, r a , r b , d a , d b , (cid:96), c ), wherein: – W is the free space, – O is the set of obstacles, – r a ∈ W is the initial position of a , – r b ∈ W is the initial position of b , – d a ∈ W is the goal or destination of a , – d b ∈ W is the goal or destination of b , – (cid:96) ∈ R + is the length of the tether, – c : I → W is the initial arrangement of the cablein W , where c (0) = r a , and c (1) = r b , and L [ c ( s )] := (cid:90) I c ( s ) ds ≤ (cid:96). A solution to trpmpp ( W , O, r a , r b , d a , d b , (cid:96), c ) is apair of paths ( τ a , τ b ) in which: – τ a : I → W where τ a (0) = r a and τ a (1) = d a , and – τ b : I → W where τ b (0) = r b and τ b (1) = d b , and – for τ a and τ b , there exists a W -feasible motion foran (cid:96) -length cable, as formalized next. Definition 2 ( W -feasible motion for an (cid:96) -lengthcable) In the closed subset of the plane W ⊆ R , fortwo paths τ a , τ b : I → W and length (cid:96) ∈ R + , a function c : I × I → W is called a W -feasible motion for an (cid:96) -length cable if and only if the following three conditionshold: c-i (The cable connects the robots) ∀ s ∈ I : c ( s,
0) = τ a ( s ) and c ( s,
1) = τ b ( s ) , c-ii (Cable has bounded length) ∀ s ∈ I : L [ c s ( x )] ≤ (cid:96), where c s ( x ) := c ( s, x ) , c-iii (Continuity) c is continuous with respect to the in-duced topologies.A visual example that serves as a quick summary ofthe preceding definitions appears in Fig. 2. We find it convenient to use L [ f ] to denote arc length offunctions f : I → W throughout this work. r a r b d a d b v v v v v v v Fig. 2: The green line segment is the initial cable con-figuration and the green dashed line segment is the finalcable configuration. The arrows show the distance op-timal path for each robot.
Definition 3 (Distance optimality)
A solution pair( τ (cid:63)a , τ (cid:63)b ) for trpmpp ( W , O, r a , r b , d a , d b , (cid:96), c ) is called distance optimal if all other solutions ( p, q ) havemax ( L [ τ (cid:63)a ] , L [ τ (cid:63)b ]) ≤ max ( L [ p ] , L [ q ]) . This particular optimality metric models minimumenergy consumption between the two robots. We willpresent a method that finds a distance optimal solutionto a trpmpp . In Section 6 we show that solutions underthis distance metric can yield optimal solutions undera time-based criterion too.
We prove, first, that it suffices to have robots a and b move on a straight line from one vertex to another in thereduced visibility graph (RVG). (Though our interest isin characterizing the solution set, this is potentially alsogood news in terms of the sensing technology needed forthe robots to be able to execute optimal plans, cf. Tovaret al. (2007).) The following theorem and widely knowncorollary indicate that, for a single robot, the shortestpath between two points in W is a concatenation of linesegments that are edges of the RVG. Theorem 1
There exists a semi-free path between anytwo given points p and q if and only if there exists asimple polygonal line T lying in W whose endpoints are p and q , and such that T ’s vertices are in verts( O ) .(Latombe, 1991; De Berg et al., 2008) More useful for us the statement below which, thoughonly stated informally by LaValle (1999), follows di-rectly the previous result.
Reza H. Teshnizi, Dylan A. Shell(a) The three paths used in the cat op-erator. (b) Choosing a value for t gives a curve. (c) Choosing a value for s gives a pointof the curve. Fig. 3: A visual explanation of how three paths are connected via the cat operator.
Corollary 1
The shortest path for a robot from onepoint to another in a subset of R can be found bysearching the shortest path roadmap or reduced visibilitygraph. The list of RVG vertices π a = ( v a , v a , . . . , v n − a , v na )connecting initial and destination positions, i.e. , with r a = v a and d a = v na , is easily turned into a curve, τ a ( s ), by joining line segments connecting v i − a to v ia sequentially, head to tail, and parameterizing appropri-ately via I . Hence, a pair of sequences of RVG verticesfor the two robots, ( π a , π b ), suffices to give a pair ofpaths ( τ a , τ b ).Although the preceding classical results hold for in-dividual robots, when two robots are constrained suchthat the action of one limits the actions of the other (asin the case of a tether of finite length), then it is lessclear that the discrete structure of the RVG encodesan optimal solution. One might conceive, in the tworobot setting, one robot deviating from visibility edgesin order to enable the other to move. A main result ofthe paper (Theorem 2), and the basis for the algorithmwe present, establishes that no such deviations are nec-essary. Indeed, the RVG still suffices to find optimalsolutions. More formally, let Π a be the set of all pathsfrom r a to d a and let Π b be the set of all paths from r b to d b in the RVG. Proof of Theorem 2, the fact that( τ (cid:63)a , τ (cid:63)b ) ∈ Π a × Π b , is via several steps. The largest sin-gle intermediate step is in establishing Lemma 2, on thefollowing page, but it requires a few definitions first.To start, in what follows, we can think of an alwaystaut tether. This is without loss of generality because:(1) tightening a tether is a continuous operation, so itpreserves homotopy; (2) if the cable length constraint issatisfied for a taut tether, it must be for others as well.The reader should bear in mind that the taut tether ismerely a special representative of the homotopy classof tethers. It is helpful to have an operator to give thisrepresentative: Definition 4 (Tightening Operator)
Given a path α : I → W , we define the operator ˆ · : ( I → W ) → ( I → W ) such that ˆ α is the (unique) shortest path inthe homotopy class of α .Practically, the classical algorithm of Hershbergerand Snoeyink (1994) is used to obtain the shortest pathhomotopic to a given path. The tightening operator willbe used, in what follows, on paths and also on cableconfigurations. For the latter, the following definitionhelps. Definition 5 (Connecting Paths via a Cable)
Giventwo paths τ : I → W and τ : I → W , and a ca-ble configuration c : I → W , we define a function thatconcatenates τ , τ , and c . Letcat( s ; t, τ , τ , c ) := τ ( t − st ) 0 ≤ s ≤ c (3( s − )) < s < τ (3( s − ) t ) ≤ s .Then, for any fixed t ∈ I , argument s ∈ I parameterizesa new curve comprising those portions of τ and τ upto t connected via c .Since both s and t are curve parameters, the defini-tion of cat can appear abstruse at first. Fig. 3 helps byillustrating how three given paths are joined via cat .Definition 1, describing the planning problem, re-quires paths for which a feasible trajectory exists. Theconnection between paths and trajectories is, of course,a timing. Thus we need the following concept: Definition 6 (Re-parameterization) A re-parameterization is a monotonically increasing,continuous function r : I → I with r (0) = 0 and r (1) =1. A pair ( r , r ) is a re-parameterization pair if bothfunctions, r and r , are re-parameterizations. otion Planning for a Pair of Tethered Robots 5 With the preceding scaffolding, we can next give adefinition that is valuable in helping to identify the exis-tence of a feasible trajectory given paths. The notation ◦ is for function composition, i.e., τ ◦ r ( t ) = τ ( r ( t ))and τ ◦ r ( t ) = τ ( r ( t )). Definition 7
Given two paths τ : I → W and τ : I → W , and a cable configuration c : I → W , let C (cid:63) ( τ , τ , c ) :=min ( r ,r ) over allre-parameterization pairs (cid:18) max t ∈ I L (cid:104) (cid:99) cat( · ; t, τ ◦ r , τ ◦ r , c ) (cid:105)(cid:19) . Informally, C (cid:63) gives the shortest cable that permitsone to execute τ and τ . The definition of C (cid:63) resem-bles the Fr´echet distance (Ewing, 1969). The key dif-ference is that C (cid:63) takes in a cable configuration c andobtains the measure subject to the path connecting thetwo curves being constrained to a particular homotopyclass. That homotopy class is prescribed via cable con-figuration c . (See also discussion in Section 8.)The intuition which connects with the main result isthat any solution ( τ a , τ b ) to a given trpmpp must havea C (cid:63) ( τ a , τ b , c ) ≤ (cid:96) . We would like to establish that anyoptimal solution to a given trpmpp can be related tosome solution on the RVG ( i.e. , in Π a × Π b ) that, whilebeing distance optimal, also abides by the conditionsimposed by cable. Corollary 2 states this formally, butwe need the following lemmas to get there. Lemma 1
Path pair ( τ a , τ b ) is a solution to a given trpmpp , iff C (cid:63) ( τ a , τ b , c ) ≤ (cid:96) .Proof. For a contradiction assume C (cid:63) ( τ a , τ b , c ) > (cid:96) and reach a contradiction with c-ii in Definition 2.The following lemma establishes an important factabout consumption of cable as the robots move on shortpaths. The result states that the consumption is a con-vex function, which leads to a practical way to verifythat a pair of paths do not violate the limits imposed bythe bounded cable, i.e., it suffices to check the lengthsat the two end points. Lemma 2
Let (cid:98) τ a and (cid:98) τ b , being the shortest paths intheir respective homotopy classes, be the prescribed pathsfor robots a and b . Then the length of the consumed ca-ble is a convex function, if (cid:98) τ a and (cid:98) τ b use the same curveparameter.Proof. Because both (cid:98) τ a and (cid:98) τ b are the shortest pathsin their respective homotopy classes, we can assert thatthey comprise sequences of pairwise connected straightline motions which lie on the RVG edges (followingCorollary 1). Moreover, each straight line motion is ofone of the following type: F : the motion is on the same edge as the cable and is[F]ollowing the cable, or L : the motion is on the same edge as the cable and is[L]eading the cable, or O : is any [O]ther straight line motion.We will argue that the length of cable consumed as arobot moves along such trajectories is a convex functionby showing that the gradient of the function is mono-tonically increasing. A technical difficulty with cableconsumption is that it is a continuous but only piece-wise differentiable function. In circumstances where thederivative is undefined, we take the value of the deriva-tive from the right. These circumstances arise whencontacts between the cable and obstacles are made orbroken. The cable consumption is a function of twocurve parameters: so by derivative we are referring tothe partials with respect to each parameter—one foreach robot.When the robot is moving on an F (or L ) segmentthe derivative is − O segments the function might have multiple pieces.For each piece of each such segment, the pieces are re-gions where the points that the cable contacts are un-changed. Then, with a single robot moving, only the ca-ble between the robot and the closest contact point con-tributes to the change in cable consumption. Writingthe consumption as a function of the curve parameterand simply taking its derivative results in a monoton-ically increasing function whose value is in the ( − , (cid:98) τ a and (cid:98) τ b have a specific struc-ture: each trajectory is of the form F ∗ O ∗ L ∗ (wherewe have used regular expression notation with Kleenestars). Showing the following suffices:(a) no O segment is followed by an F segment, and(b) no L segment is followed by an O segment, and(c) no L segment is followed by an F segment.For proof of (a): Assume there exists a motion, (cid:98) τ ,in which segment A of type O ending at vertex v isfollowed by an F segment, starting at v . As we mayassume the cable is always taut, we can assert that itlies on the shortest path to v . Thus, replacing A with asegment/segments in which the robot follows the cableto arrive at v yields a shorter path than (cid:98) τ . This is acontradiction as (cid:98) τ is the shortest path in its homotopyclass. Reza H. Teshnizi, Dylan A. Shell v v v v (a) A scenario illustrating the cable consumption function.Originally the cable is in contact with v and v . As therobot travels towards its destination (to the right), it willrelease contact with v . It will then make contact with v and v , respectively. The dotted lines represent places inwhich cable events occur. Curve parameter C o n s u m e d c a b l e (b) Length of consumed ca-bled as a function of curveparameter. Curve parameter D e r i v a t i v e (c) Derivative of lengthof consumed cabled perchange in curve parameter. Fig. 4: An illustrative example showing how, for a straight line motion, the cable consumption is a convex function.As the robot (in Fig. 4a) makes its way towards the destination, the cable will make or release contact with elementsof verts( O ). These events change the derivative of the consumed cable. Color coding of continuous motion helpsshow how the function’s pieces are separated by the occurrence of discrete cable events. Fig. 4b and 4c show thefunction and its derivative, respectively. v v v v v v v v v r a r b d b d a (a) The original configuration of the cable. v v v v v v v v v r a r b d b d a (b) The optimal solution to the given trpmpp which is foundby searching the search tree. Fig. 5: An example scenario for which we illustrate part of the search tree in Fig. 6Similarly, proof of (b): Suppose there exists a mo-tion, (cid:98) τ , in which an L segment ending at vertex v isfollowed by segment B of type O starting at v and end-ing at v (cid:48) . Given that the robot is leading the cable andthat we may assume the cable is always taut, the cablelies on the shortest path from v to v (cid:48) . Again, replacing B with a segment/segments in which the robot followsthe path of a taut cable to arrive at v (cid:48) will be shorterthan (cid:98) τ . But then (cid:98) τ can’t have minimal length withinits homotopy class.The proof for the impossibility of (c) is as follows:Tautening such a cable leads to a shorter, pure F seg-ment. The L segment provided excess length; and its re-moval reduces the distance along which the robots needto move. But this contradicts the fact that the pathsare the shortest in their respective homotopy classes.Taken together, this proves that (cid:98) τ a and (cid:98) τ b are of theform F ∗ O ∗ L ∗ . Hence, global monotonicity of the partialderivative of the cable consumption function holds if we can show that monotonicity is preserved between twoconsecutive O segments. To show no violation occursat the transition between segments, monotonicity in asmall open interval around the transition is sufficient. Ifthe two O -segments, g and g , are collinear, then theycould be treated as a single segment and the prior argu-ment for a single O -segment holds. Hence, there mustbe a ‘turn’ from segment g to g . Both segments areon the RVG so that turn occurs at a vertex v o of someobstacle. Presume that we extend and continue along g an extra (cid:15) >
0; then the derivative of the cable con-sumption continues to increase (as the single segmentargument holds). Since g is not along this little exten-sion, it falls to one side. If that side is away from thecable-obstacle contact, then the additional motion awayconsumes extra cable, so the derivative only increasesfaster. Otherwise, when turning towards the contact,monotonicity may indeed fail. However, such a turnleads to a contradiction; two cases are possible: the ob- otion Planning for a Pair of Tethered Robots 7 stacle to which v o belongs is on the inside of the turn,or it is on the outside. If it is on the inside, then thecable itself wraps around v o , in which case g is not an O segment (but an L one). If the obstacle is on theoutside, then simply shaving off a small corner at theturn is feasible. But that is shorter, contradicting thesupposition that g and g result from shortest motionsin their homotopy class.Hence monotonicity of the partial derivative of thecable consumption holds across the entire I . Now con-sider the concurrent motion of both robots: we feedthem the same curve parameter and the derivative ofthe cable consumption simply becomes the total deriva-tive. Being the sum of two partials, each of which ismonotone, gives a monotone function. Thus, the totalcable consumption is a convex upward function of thecurve parameter.The preceding does the heavy lifting for the proofof our theorem. Theorem 2 If ( τ a , τ b ) is a solution for a given trpmpp ,then ∃ ( τ V Ga , τ
V Gb ) ∈ Π a × Π b : C (cid:63) ( τ V Ga , τ
V Gb , c ) ≤ (cid:96) .Proof. Let (cid:98) τ a and (cid:98) τ b be the shortest paths homotopicto τ a and τ b , respectively. Hence, ( (cid:98) τ a , (cid:98) τ b ) ∈ Π a × Π b from Corollary 1. Then to prove this theorem it sufficesto show that the following inequality holds: C (cid:63) ( (cid:98) τ a , (cid:98) τ b , c ) ≤ (cid:96) .Since ˆ · is homotopy preserving, the following con-ditions hold: L (cid:104) (cid:99) cat( · ; 0 , (cid:98) τ a , (cid:98) τ b , c ) (cid:105) = L (cid:104) (cid:99) cat( · ; 0 , τ a , τ b , c ) (cid:105) = L [ (cid:98) c ] , L (cid:104) (cid:99) cat( · ; 1 , (cid:98) τ a , (cid:98) τ b , c ) (cid:105) = L (cid:104) (cid:99) cat( · ; 1 , τ a , τ b , c ) (cid:105) .Moreover, because ( τ a , τ b ) is a solution we have L (cid:104) (cid:99) cat( · ; 0 , (cid:98) τ a , (cid:98) τ b , c ) (cid:105) ≤ (cid:96) , and L (cid:104) (cid:99) cat( · ; 1 , (cid:98) τ a , (cid:98) τ b , c ) (cid:105) ≤ (cid:96) .Following Lemma 2, feeding the same curve param-eter to both (cid:98) τ a and (cid:98) τ b , gives a convex upward cableconsumption. Therefore,max t ∈ I L (cid:104) (cid:99) cat( · ; t, (cid:98) τ a , (cid:98) τ b , c ) (cid:105) =max t ∈{ , } L (cid:104) (cid:99) cat( · ; t, (cid:98) τ a , (cid:98) τ b , c ) (cid:105) ≤ (cid:96) .Because the above holds for the trivial identity re-parameterization, the minimum over the set of all re-parameterization pairs must be less than or equal tothe above. This fact, when combined with Lemma 1,completes the proof. Corollary 2 (Distance Optimality)
Let ( τ (cid:63)a , τ (cid:63)b ) bea distance optimal solution to a given trpmpp . Thenthe RVG also contains a distance optimal solution, ( τ (cid:63)V Ga , τ (cid:63)V Gb ) .Proof. A pair ( τ (cid:63)V Ga , τ (cid:63)V Gb ) ∈ Π a × Π b is constructedfrom ( τ (cid:63)a , τ (cid:63)b ). Simply take ( τ (cid:63)V Ga , τ (cid:63)V Gb ) = ( (cid:99) τ (cid:63)a , (cid:99) τ (cid:63)b ) andobserve the three following facts: – (cid:99) τ (cid:63)a and (cid:99) τ (cid:63)b is on the RVG, – the distance traveled along (cid:99) τ (cid:63)a and (cid:99) τ (cid:63)b for each robotis no worse following Corollary 1, – C (cid:63) ( (cid:99) τ (cid:63)a , (cid:99) τ (cid:63)b , c ) ≤ (cid:96) following Theorem 2. In this section we present an implementation of A (cid:63) (Hart et al., 1968) to construct and explore the searchtree for the solution to this problem. Consider a trpmpp with ( W , O, r a , r b , d a , d b , (cid:96), c ). The search tree’s nodesrepresent cable configurations. An edge between twonodes represents a motion for each robot along an RVGedge that abides by the cable requirements. To do so,we define a data-structure with the following fields: – taut cable : a list of RVG vertices each of which is vis-ible from the previous ones describing a valid cableconfiguration, – cost : a pair of costs indicating the distance traveledby each robot to arrive at this cable configurationfrom their initial poses, – a reference to a parent node.Fig. 5 provides an example scenario, and Fig. 6 givesthe structure of the search tree for this scenario. UsingA (cid:63) search, Algorithm 1 explores the search tree —whichgives a structured representation to the set Π a × Π b —for an optimal solution to a given trpmpp . Being a typeof informed search, A (cid:63) uses an estimated cost computedas the sum of a cost function and an heuristic func-tion. The cost is calculated cumulatively with a con-stant time operation when creating/updating a node.When using an admissible heuristic function, we cansafely terminate the search in a branch whose estimatedcost is higher than the best solution found (Russell andNorvig, 2010). In Sec. 7.3 we examine the effect of dif-ferent admissible heuristics on the efficiency of the al-gorithm.Given any node in the tree, we can obtain a pair ofpaths —one for each robot— that continuously trans-forms the original cable configuration to the configura-tion stored in the node. To do so, we can traverse thetree from the root down to the node. Start with π a and Reza H. Teshnizi, Dylan A. Shell ...... (a) A visual representation of the searchtree, with vertices bearing labels that areiconic depictions of the cable configura-tions; see (b)–(d). (b) Two of the con-figurations that can beachieved from the con-figuration stored in theroot node. (c) Both cables shownhere can be achievedfrom the cyan one in theprevious snapshot. Or-ange is preferred as it re-duces the distance trav-eled by robot a . (d) The final configura-tion to arrive at the des-tination node. Fig. 6: A small portion of the search tree for the trpmpp in Fig. 5. The optimal solution to the given trpmpp inthis figure can be obtained by traversing the node containing the brown cable configuration up to the root.
Algorithm 1: A (cid:63) over Cable Configurations Search ( W , O, r a , r b , d a , d b , (cid:96), c )2: Build RVG, g , with vertices { verts( O ) , r a , r b , d a , d b }
3: root = Node( (cid:98) c , ∅ )4: priorityQ = { root } while priorityQ is not empty do
6: n = priorityQ.dequeue()7: if n.cable.first = d a and n.cable.last = d b then if L [n.cable] ≤ (cid:96) then return constructed path by following parentreferences from n up to the root10: else continue end if end if V a = g. visibleVerts(n.cable.first)15: V b = g. visibleVerts(n.cable.last)16: for all ( v a , v b ) ∈ V a × V b do c = [ v a ]+ n.cable +[ v b ]18: child = Node( (cid:98) c , n)19: priorityQ.enqueue(child)20: end for end while π b as two empty sequences. While traversing the treedown to the node, for each node we take the first andlast element of the taut tether and append it to the endof π a and π b , respectively. Notice that the sequences π a and π b will have equal cardinality. In fact, we seethat the taut cable list at each vertex operates like adeque. Each robot, by making continuous motions inthe plane, can modify the cable. The important quali-tative changes to the cable are cable events: wherein anobstacle vertex is added or deleted. The physical prop-erty of the cable means that each robot’s motion canonly push or pop at its respective end of the cable. Theorem 3 (Soundness)
A pair of paths, ( τ a , τ b ) ,generated by traversing the above mentioned search treefrom the root to a leaf is a solution to the corresponding trpmpp , if the leaf contains a taut tether configurationwhose end points are d a and d b . Proof. The root contains the taut tether ˆ c , which bydefinition has end points r a and r b . Because the algo-rithm checks whether a leaf contains a taut tether con-figuration whose end points are d a and d b on line 7,the requirements τ a (0) = r a , τ a (1) = d a , τ a (0) = r a ,and τ a (1) = d a is satisfied. Using the result of Theo-rem 2, the check on line 8 ensures ( τ a , τ b ) satisfies thefinal requirement for a solution. Theorem 4
Algorithm 1 is complete.Proof.
Completeness is shown by considering complete-ness of A (cid:63) search, and Corollary 2 that indicates thatthe appropriate structure generated from the RVG isthe solution space.
In Section 3 we defined a distance optimal solution.Solving a trpmpp only requires the existence of a W -feasible motion for an (cid:96) -length cable. This abstractsaway details of time and allows us to consider a geo-metric problem, i.e., one on paths. But actually mov-ing the robots requires trajectories. This last step isimportant to solve the problem we have laid out: wehave proven the existence of some timing for the pathsreturned by the algorithm, but some timings (indeedmany of them!) can violate the cable constraint duringexecution. Thus, next, we prescribe a suitable timing.Informally, an execution for a tethered pair of robotsis a function that maps a trpmpp solution ( i.e. , a pairof paths) to a pair of trajectories (as defined by Latombe(1991)), one for each robot. We assume robots a and b both have maximum speeds of mv and that turning isinstantaneous. For simplicity, we further assume thatthe robots can achieve speed mv immediately.For a given a pair of paths ( τ a , τ b ), define T = max (cid:26) L [ τ a ] mv , L [ τ b ] mv (cid:27) , ( † ) otion Planning for a Pair of Tethered Robots 9 that is, T is the time which it takes for the robot that,moving at maximum speed throughout, arrives at itsdestination second. Then we construct trajectories Υ a :[0 , T ] → W and Υ b : [0 , T ] → W as follows Υ a ( λ ) = τ a (cid:18) λT (cid:19) and Υ b ( λ ) = τ b (cid:18) λT (cid:19) . ( ‡ )Note that by construction neither robot exceeds themaximum velocity.Now, with notation for trajectories, the time indexmeans we can define a temporal notion of optimality. Definition 8 (Time optimality)
A pair of trajecto-ries ( Υ (cid:63)a , Υ (cid:63)b ) that are executions for a trpmpp solutionis called time optimal if no other trajectories have both a and b arriving at d a and d b sooner than the later ofthe two under Υ (cid:63)a and Υ (cid:63)b .Leading to the following final claim: Theorem 5
Given a distance optimal solution ( τ a , τ b ) returned by Algorithm 1, the pair of trajectories con-structed by ( † ) and ( ‡ ) constitutes a time optimal exe-cution.Proof. Algorithm 1 produces distance optimal solutionssuch that ( τ a , τ b ) = ( (cid:98) τ a , (cid:98) τ a ). Then Lemma 2 providesthat the trajectories ( t a , t b ) abide by the cable con-straints. The distance metric searches for minimum ofthe maximum distance traveled by either robots andthe total time taken, given by ( † ), is the fastest therobots could travel that distance. To demonstrate this algorithm, we implemented it inPython (v3). Our implementation makes heavy use ofThe CGAL Project (2020) for computational geometryalgorithms, namely convex hull and triangulation meth-ods. Fig. 8 shows a screenshot of the user interface.7.1 Empirical Assessment of the Method: aComparisonOur earlier workshop paper (Teshnizi and Shell, 2014)proposed a dynamic programming (DP) approach formotion planning for a tethered pair. The approach uses,as a sub-procedure, a planner for the motion of a singlerobot connected to a fixed base with a bounded tether.The method in that work does not rely on any specificsingle robot planner, as long as it can return as outputthe cost and the length of the tightened cable. As statedin that paper, the DP technique only solves instances of trpmpp of a particular form: it will only return motionsof the robots that retain at least one contact point onthe cable throughout the execution. Hence, it is nota complete algorithm.Meaningful comparison of Algorithm 1 and the DPapproach requires that we select a problem for whichthe latter will return a solution. The scenario shown inFig. 9 meets this criterion and it was the setting used forour empirical assessment. To remove the impact of im-plementation details on performance, we implementedthe DP technique in Python using a specialized trpmpp planner setup to solve for a single robot.Here, for a heuristic function, we use the straightline (Euclidean) distance (which we denote SLD) be-tween the robots and their destinations. This can becomputed in constant time. We measured the perfor-mance in terms of the number of search tree nodes ex-panded and generated as well as wall time. Table 1 andFig. 10 present these metrics. It is obvious that A (cid:63) for-mulation outperforms DP.Table 1: Comparison of the performance between DPversus A (cid:63) formulations on the scenario in Fig. 9.
Algorithm Expanded Generated Wall Time (s)DP 41 118 4.52A (cid:63) (cid:96) = 470 and again at (cid:96) = 525. The steps arequalitative changes in solutions occurring as the tetherbecomes long enough to permit a new homotopy classof solutions. Observing Fig. 12 one sees that the stepshappen as the length of the consumed cable in the op-timal solution equals (cid:96) exactly. Another fact is that checking whether two paths, one foreach robot, will satisfy the cable length constraint when ex-ecuted concurrently depends on the speeds at which theymove; hence, that paper ought to have made some such as-sumption, as done in Section 6.0 Reza H. Teshnizi, Dylan A. Shell(a) (cid:96) = 200 and (cid:96) = 300 (b) (cid:96) = 400 (c) (cid:96) = 500 (d) (cid:96) = 700
Fig. 7: Solving the same scenario with different cable length.Fig. 8: A screenshot of user interface of our implemen-tation. The red and blue circle filled circles representthe robots. Obstacles are in grey. The initial and finalconfiguration of the cable are shown in green and darkred. The prescribed path for each robot is shown withlines of the same color. The RVG vertices where for eachrobot is shown with small dots along the path.Every solution for a cable of length (cid:96) is a solutionfor longer cables too. But if longer cables mean largersolutions spaces, they also mean larger search spaces.One might expect, therefore, that solving problems withlonger cables would take more time. Fig. 13 shows thenumber of search nodes for cables ranging in lengthfrom 200 to 700. Observe that there is a steady increasewith the algorithm exploring a subset of a search spacewhich is growing, from 200–350. But then the cablebecomes long enough for the search to terminate muchsooner. The pattern is repeated a few times. What hap- Fig. 9: The scenario used for comparison between Dy-namic Programming and A (cid:63) formulations.pens is that the heuristic pulls the search towards partsof that space which, though promising, are not fruit-ful because the topological constraint will ultimatelymake them dead-ends. With a longer cable, branchesthat moved in the right direction but then turned outto be infeasible, now become feasible.7.3 The Effect of Informed SearchThe topology of the environment as well as the ca-ble length affects both the number of expanded and otion Planning for a Pair of Tethered Robots 11
Fig. 10: A comparison between the performance of Dy-namic Programming vs. the A (cid:63) formulation. In eachbar, the light and dark color represent the number of ex-panded and generated search nodes for each algorithm.The bars are stacked as expanded nodes are a subset ofthe generated ones.Fig. 11: The distance traveled by each robot in optimalsolution for different maximum cable lengths for sce-nario shown in Fig. 7. The constraint that the plannerattempts to optimize is highlight for each cable length.generated nodes in the search. Making an appropri-ate choice of heuristic function for A (cid:63) can help. In theprevious sections, we reported results from the simpleSLD heuristic. This heuristic is na¨ıve, completely dis-carding topological and obstacle constraints, but stillimproves the efficiency of the search substantially. Weestablished this fact via a comparison to a base-line. Weimplemented Uniform Cost Search (UCS) which yields Fig. 12: The length of the consumed cable in optimal so-lution for different maximum cable lengths for scenarioshown in Fig. 7Fig. 13: The number of expanded vs. generated nodesfor different maximum cable lengths for scenario shownin Fig. 7.an optimal output but without any forward estimates.Fig. 14 shows the impact of the heuristic information issignificant.More sophisticated heuristics, that consider morethan simple Euclidean distance, can help reduce thesearch time further. Specifically, we considered two otherheuristic functions, each being lesser relaxations of theproblem: – Shortest Path Distance (SPD) respects obstacles: itis the length of the shortest path from a robot’s po-sition vertex to its destination. We compute this by running A (cid:63) on the single (untethered) robot prob-lem, itself using SLD. – trpmpp Jr. respects obstacles and some topologicalconstraints. It is computed by running a version ofthe problem instance at hand by but with a longermaximum cable, with SPD. (It is so named becauseit considers a diminished version of trpmpp .)For trpmpp
Jr., we sought to develop a heuristicthat considers some aspects of the cable’s configura-tion. The results described in previous section showthat searches with longer cables tend to be faster, so itis plausible that trpmpp
Jr. could be computed quicklyand provide high-quality information to guide the searchfor the original trpmpp . Based on those results, it usesa cable length multiplier, as a function of the numberof vertices on the taut tether, to allow faster compu-tation for more complex cable configurations. Notice,also, that SPD can be thought of as trpmpp
Jr. butwith (cid:96) = ∞ . Hence, trpmpp Jr. dominates SPD, whichin turn dominates SLD.As is apparent from Fig. 15, these heuristics doreduce the number of nodes expanded and generatedby the search, but the improvements—at least in thisscenario—are minuscule. Another important consider-ation is that, while SLD is a constant time operation,SPD and trpmpp
Jr. usually require substantial com-putation. When we compare wall times, in Fig. 16, SPDoffers some small improvement, whereas trpmpp
Jr.has no discernible value. Both SPD and trpmpp
Jr.involve searching relaxed problems and, in light of Val-torta’s Theorem (Valtorta, 1984), it is perhaps unsur-prising that the total number of nodes are so similar(visible in Fig. 15).
Next we further consider the relationship of the presentpaper with respect to Teshnizi and Shell (2014); thatpaper defined and examined a useful way of decompos-ing the c-space of a single tethered robot. It constructeda tree structure that formed a skeleton that was relatedto an atlas representation of the c-space manifold. Given the inputs to the single robot problem, one vi-sualizes its c-space most easily by sketching the chartsand showing where they touch (see Fig. 17). For thesingle tethered robot, this is (locally) a 2 dimensionalobject, and consequently easy to see. This structure al-lows for the convenient and simultaneous representation Technically one must consider a manifold with a bound-ary, and the notion of ‘chart’ here may be closed set; forsimplicity throughout we will ignore these nuances.
Fig. 14: A comparison of number expanded and gener-ated nodes between a Uniform Cost Search (UCS) vs.A (cid:63) with Straight Line Distance (SLD) heuristic. Thelight and dark colors are stacked bars, representing ex-panded and generated nodes respectively.Fig. 15: A comparison of number expanded and gen-erated nodes between A (cid:63) with three different heuristicfunctions: Straight Line Distance (SLD), Shortest PathDistance (SPD), and trpmpp
Jr. The light and darkcolors represent expanded and generated nodes.of two different pieces of data: the motion of the robotthrough space, and the homotopy class of the cable.The single tethered robot problem is a special caseof tethered pairs where one robot remains stationary:requiring only 2 degrees of freedom. For a general teth-ered pair, a comparable figure would require four di-mensions, impeding easy visualization. In Fig. 18 wehave moved the base of the tether slightly to the rightto show the changes in the charts. The boundaries ofthese charts are 2D surfaces in a 4D space.When there is some vertex where the cable makescontact, it is helpful to imagine two trees rooted at thatpoint. Fig. 19 provides a view of this. From the per-spective of our search tree, cable-obstacle contact that otion Planning for a Pair of Tethered Robots 13
Fig. 16: A comparison of computation wall time be-tween A (cid:63) with three different heuristic functions:Straight Line Distance (SLD), Shortest Path Distance(SPD), and trpmpp
Jr.forms the common root is an element within the deque.(Recall the nodes of the search tree have cable configu-rations expressed as lists of vertices, which operate likea deque.) The subtrees on either side are representedby lists, via pushing and/or popping, that retain thatelement.The robots share a total amount of cable, so as oneeither consumes or gives up cable, the depth of the treeor the circular regions circumscribing the outer limitsof the other robot reduce or increase, respectively. Oneattractive aspect of this mental picture is how the ca-ble describes a tree quite obviously: each time the ca-ble wraps around an obstacle, some cable is consumedand it is as if it is anchored at that wrapping point.The recursive nature is clear because one obtains a newproblem, similar to the previous one, but ‘smaller’ inthe sense of available cable. This point of view was themain idea presented by Teshnizi and Shell (2016b).If the robots start popping cable contacts out of thedeque, moving up the tree (e.g., by following F segmentafter F segment) eventually the cable-obstacle contactthat forms the common root is popped. At this pointthe node deque contains only the two robots and phys-ically the robots must be directly visible to one an-other. They can then move and, stringing the tetherbetween them, make a contact with another obstaclevertex. Doing so, leads back to a Fig. 19-like scenario.Hence, we can see that this top-level portion of c-spacehas many trees hanging like tendrils off of it. The top-level portion of c-space unlike the sort of charts madeof pieces of spirals as depicted above, is truly 4D andcomprises sub-regions where the pair of robots main-tain line-of-sight. Consider that the reduced visibilitygraph, as a discrete structure, describes where tendrils A B C D
Tether's base
Obstacle
Fig. 17: Some of the charts of the atlas representing thec-space of a tethered robot. A , B , C , and D are fourcharts in the atlas. In the illustration, gray lines markthe chart boundaries. A ' B ' C ' Fig. 18: Moving the base of the tether leads to a changesin the boundaries of the charts. Here A (cid:48) , B (cid:48) , C (cid:48) are thenew charts. The charts prior to moving the base areillustrated in light blue for comparison.can be found. But, further, the reduced visibility graph,as embedded in the plane, gives a way to think aboutthe 4D chart if we restrict motions to the graph edges:within this space, the robots are either on the same ver-tex, or one edge away from each other. When restrictedto paths on the visibility graph, within the 4D chartthe robots must move on a sort of abstract boundaryas they leapfrog and never get too far from one another,any deviation therefrom forming a tendril.Previously, we said that our definition of C (cid:63) resem-bles the classic Fr´echet distance, but pointed out thatit differs because C (cid:63) respects the homotopy class ofthe paths being considered. Adopting the perspectiveof the c-space manifold, if τ and τ are curves not in Cable-obstacle contactRobot a Robot b (a) The configuration of the robots and their cable. A B (b) The visibility chartsfor robot a . A'B' (c) The visibility charts forrobot b . Fig. 19: A snapshot of the conjoined atlases for a teth-ered robot pair after the first contact is made betweenthe cable and an obstacle. W , but instead in the 4D c-space, then C (cid:63) becomes theFr´echet distance. Our algorithm operates on paths inthe workspace W , which one might think of as projec-tions from the 4D configuration space. Hence, to recoverenough information to compute the distance requiresthe cable configuration in addition to τ and τ .Prior work (such as our own (Teshnizi and Shell,2014) and also others) has recognized the structure ofthe tree-like discrete skeletons that describe the atlasfor a fixed tether point. Most would recognize thatpushing and popping (via physical motions over crit-ical events in terms of visibility) only occurs at the freeend of the cable, so the data-structure is essentially astack. So in summary: a 2D c-space gives a stack, a4D c-space gives two stacks joined end-to-end, a deque.We are not aware of a more general realization thattwo robots give rise to a discrete structure mirroring adeque, nor that the case of the empty structure has spe-cial significance. One observation that is plain from thisdiscrete data-structure point of view is that there canbe multiple ways to arrive at the same deque contents.Indeed, this underscores the fact that the tethered paircase occurs on a manifold with cycles within it; cyclesthat are absent for an anchored tether. Hence, the ten-drils are woven together and one can ‘pick them up’ tore-root from several places. This work presents, to the best of our knowledge, a firstalgorithm for planning distance optimal motions for a pair of tethered robots. The finite cable length makesthis problem different compared to work in the liter-ature on tether pairs: prior work focuses on planningmotions for a robot pair that need only consider topo-logical constraints. In such cases, challenge becomes oneof representing topological properties, via signatures orother techniques. In our case, it has turned out that thechallenge was not the topological element of the prob-lem, but rather the metric one that arises when consid-ering a cable of bounded length. That is, determiningif a path pair is a solution to a given trpmpp requiresensuring existence of a feasible trajectory for the cableof bounded length. The very question of whether solu-tions can be found in Π a × Π b (i.e., the RVG) is farmore obvious when one robot’s motion only constrainsthe other’s topologically. It is not hard to see that tim-ing (i.e., the robot’s motions relative to one another)becomes coupled and the problem involves dealing withsome sort of coordination. Even glancing at the paperit is clear that greater care than is typical was neededto define what constituted a solution to a tethered pairmotion planning problem so as to encode the finitenessof the cable.Fortunately, shortest paths on an RVG help us avoidconsideration of curve parameterizations: they have mono-tonically increasing derivative of cable consumption.Through this property, we can consider a much reducedset of solutions without losing completeness or optimal-ity. Further, this provides an elegant representation ofthe search tree; one containing different cable configu-rations up to homotopy. We start by adding the initialconfiguration to the root node and iteratively createthe configurations. Once we find a cable configurationthat has the two robots’ destinations at its ends, onlythen we need to check whether that configuration ispermitted but the length of the cable. Finally, usingthe properties of the solutions that our algorithm giveswe are able to give an optimal execution. One that alsominimizes the arrival time for the two robots.The algorithm provided in this work enumerates allcable configurations that take the robots to their re-spective destination from least cost to most cost. Weinvestigated the possibility of early termination in somebranches in the search tree. However, our results havenot yet provided adequate evidence that this addedcomplexity improves performance consistently. Our im-mediate future work will explore the different ways toterminate the search in a branch of the search tree.Acknowledgements This work was supported, in part, by the National ScienceFoundation through awards IIS-1453652 and IIS-1849249. Fig. 1,otion Planning for a Pair of Tethered Robots 15whose author is Cactus26, is used without any further ed-its under Creative Commons Attribution-Share Alike 3.0 Un-ported license . References
De Berg M, Cheong O, Van Kreveld M, OvermarsM (2008) Computational geometry: Algorithms andapplications, 3rd edn. Springer Berlin Heidelberg,Berlin, Heidelberg, DOI 10.1007/978-3-540-77974-2, arXiv:1011.1669v3
Ewing G (1969) Calculus of Variations with Applica-tions. W. W. Norton, URL https://books.google.com/books?id=oiUiAAAAMAAJ
Gooding D, Martin JD (2014) Know The Ropes: SnowClimbing. Am Alp J URL http://publications.americanalpineclub.org/articles/13201212891/Know-The-Ropes-Snow-Climbing
Hart PE, Nilsson NJ, Raphael B (1968) A Formal Ba-sis for the Heuristic Determination of Minimum CostPaths. IEEE Trans Syst Sci Cybern 4(2):100–107,DOI 10.1109/TSSC.1968.300136Hershberger J, Snoeyink J (1994) Computing mini-mum length paths of a given homotopy class. Com-put Geom 4(2):63–97, DOI 10.1016/0925-7721(94)90010-8Huntsberger T, Stroupe A, Aghazarian H, Garrett M,Younse P, Powell M (2007) TRESSA: Teamed robotsfor exploration and science on steep areas. J F Robot24(11-12):1015–1031, DOI 10.1002/rob.20219Igarashi T, Stilman M (2010) Homotopic Path Planningon Manifolds for Cabled Mobile Robots. In: Algorith-mic Found. Robot. IX, Springer, Berlin, Heidelberg,pp 1–18, DOI 10.1007/978-3-642-17452-0 1Kim S, Bhattacharya S, Heidarsson HK, Sukhatme G,Kumar V (2013) A Topological Approach to UsingCables to Separate and Manipulate Sets of Objects.In: Robot. Sci. Syst.Kim S, Bhattacharya S, Kumar V (2014) Path planningfor a tethered mobile robot. In: Proc. - IEEE Int.Conf. Robot. Autom., IEEE, pp 1132–1139, DOI10.1109/ICRA.2014.6906996Kim YH, Shell D (2018) Bound to help: cooperativemanipulation of objects via compliant, unactuatedtails. Autonomous Robots—Special Issue on Dis-tributed Robotics: From Fundamentals to Applica-tions 42:1563–1582Latombe JC (1991) Robot Motion Planning. SpringerUS, Boston, MA, DOI 10.1007/978-1-4615-4022-9LaValle SM (1999) Planning Algorithms. CambridgeUniversity Press https://creativecommons.org/licenses/by-sa/3.0/deed.en McCammon S, Hollinger GA (2017) Planning and exe-cuting optimal non-entangling paths for tethered un-derwater vehicles. In: 2017 IEEE Int. Conf. Robot.Autom., IEEE, pp 3040–3046, DOI 10.1109/ICRA.2017.7989349McGarey P, MacTavish K, Pomerleau F, Barfoot TD(2017) TSLAM: Tethered simultaneous localizationand mapping for mobile robots. Int J Rob Res36(12):1363–1386, DOI 10.1177/0278364917732639McGarey P, Yoon D, Tang T, Pomerleau F, Barfoot TD(2018) Developing and deploying a tethered robot tomap extremely steep terrain. J F Robot 35(8):1327–1341, DOI 10.1002/rob.21813Nesnas IA, Matthews JB, Abad-Manterola P, BurdickJW, Edlund JA, Morrison JC, Peters RD, TannerMM, Miyake RN, Solish BS, Anderson RC (2012)Axel and DuAxel rovers for the sustainable explo-ration of extreme terrains. J F Robot 29(4):663–685,DOI 10.1002/rob.21407Russell SJ, Norvig P (2010) Artificial intelligence : amodern approach, 3rd edn. Prentice Hall, DOI 10.1017/S0269888900007724Shnaps I, Rimon E (2013) Online Coverage by a Teth-ered Autonomous Mobile Robot in Planar UnknownEnvironments. In: Proc. Robot. Sci. Syst., Berlin,GermanyStenning B, Bajin L, Robson C, Peretroukhin V, Os-inski GR, Barfoot TD (2015) Towards AutonomousMobile Robots for the Exploration of Steep Terrain.In: F. Serv. Robot. Results 9th Int. Conf., Springer,Cham, pp 33–47, DOI 10.1007/978-3-319-07488-7 3Teshnizi RH, Shell DA (2014) Computing cell-baseddecompositions dynamically for planning motions oftethered robots. In: 2014 IEEE Int. Conf. Robot. Au-tom., IEEE, Hong Kong, China, pp 6130–6135, DOI10.1109/ICRA.2014.6907762Teshnizi RH, Shell DA (2016a) Planning motions for aplanar robot attached to a stiff tether. In: 2016 IEEEInt. Conf. Robot. Autom., IEEE, vol 2016-June, pp2759–2766, DOI 10.1109/ICRA.2016.7487438Teshnizi RH, Shell DA (2016b) Some Progress TowardTethered Pairs. In: ICRA Work. Emerg. Topol. Tech.Robot., Stockholm, SwedenThe CGAL Project (2020) CGAL User and ReferenceManual, 5th edn. CGAL Editorial BoardTovar B, Murrieta-Cid R, LaValle SM (2007) Distance-Optimal Navigation in an Unknown EnvironmentWithout Sensing Distances. IEEE Trans Robot23(3):506–518, DOI 10.1109/TRO.2007.898962Valtorta M (1984) A result on the computationalcomplexity of heuristic estimates for the A ∗∗