Asymptotically Optimal Design of PiecewiseCylindrical Robots using Motion Planning

Cenk Baykal*Computer Science and Artificial Intelligence Laboratory

Massachusetts Institute of Technology, USAbaykal@mit.edu

Ron AlterovitzDepartment of Computer Science

University of North Carolina at Chapel Hill, USAron@cs.unc.edu

Abstract—In highly constrained settings, e.g., a tentacle-likemedical robot maneuvering through narrow cavities in the bodyfor minimally invasive surgery, it may be difficult or impossiblefor a robot with a generic kinematic design to reach all desirabletargets while avoiding obstacles. We introduce a design optimiza-tion method to compute kinematic design parameters that enablea single robot to reach as many desirable goal regions as possiblewhile avoiding obstacles in an environment. We focus on the kine-matic design of piecewise cylindrical robots, robotic manipulatorswhose shape can be modeled via cylindrical components. Ourmethod appropriately integrates sampling-based motion planningin configuration space into stochastic optimization in design spaceso that, over time, our evaluation of a design’s ability to reachgoals increases in accuracy and our selected designs approachglobal optimality. We prove the asymptotic optimality of ourmethod and demonstrate performance in simulation for (i) aserial manipulator and (ii) a concentric tube robot, a tentacle-like medical robot that can bend around anatomical obstacles tosafely reach clinically-relevant goal regions.

I. INTRODUCTION

In a cluttered environment, the ability of a robotic manipu-lator to reach desired targets while avoiding obstacles dependssignificantly on the robot’s kinematic design. A robot’s kine-matic design can be seen as a set of kinematic parameters thatdefine a robot’s shape and are fixed throughout the robot’suse, e.g., the length of each link of a serial manipulator orthe lengths and curvatures of tubes in a concentric tube robot[8, 11]. In highly constrained settings, e.g., a tentacle-likerobot maneuvering through narrow cavities in the body forminimally invasive surgery, it may be difficult or impossiblefor a robot with a generic kinematic design to reach alldesirable targets while avoiding obstacles (see Figure 1).

Fortunately, advances in methods that enable the rapidfabrication of customized robot designs is introducing thepotential to create robots that are kinematically optimizedon a task-specific basis. Advances in 3D printing enable therapid creation of robots with links of customizable lengths.Customized medical robots, like concentric tube robots, can becreated in minutes by shape-setting or 3D printing [10, 22].Our objective is to computationally optimize the kinematicdesign parameters of a robotic manipulator on a task-specificbasis: given the shapes of obstacles in the environment as wellas goal regions the robot should be capable of reaching, we

*Work done as a student at University of North Carolina at Chapel Hill.

Optimal Design Generic Design

Fig. 1. Optimizing the kinematic design of a robotic manipulator can enableit to reach more goal regions in a cluttered environment. In this example,the objective is to optimize the design of a concentric tube robot, a surgicalmanipulator composed of nested, precurved tubes (cyan, yellow, magenta)whose lengths and curvatures can be customized (top). The objective isto move from a bronchial tube (white arrow) to reach goal regions (greenspheres) in the lung while avoiding anatomical obstacles, e.g., blood vessels(red, blue) and bronchial tubes (off-white). A generic design may fail toreach some goal regions in a cluttered environment (right column), while anoptimized design (left column) has the potential to reach more goal regions.

aim to compute a single robot design that can reach as manyof the goal regions as possible while avoiding obstacles.

In this paper, we specifically focus on optimizing the

kinematic design of piecewise cylindrical robots, a subclassof robotic manipulators whose shape can be modeled via aconnected sequence of cylindrical components. This modelcan be applied to standard multi-link robot arms by modelingeach link as a cylinder, where the length of each link is akinematic design parameter. Another type of robot that canbe modeled as piecewise cylindrical is the concentric tuberobot, a tentacle-like robot for minimally invasive surgerythat can curve around anatomical obstacles to reach surgicalsites in constrained spaces [8, 11]. Concentric tube robotsare composed of nested, pre-curved tubes, where each tubeis typically shaped with a straight section followed by apre-curved constant-curvature section. Each of the robot’scomponent tubes can be independently rotated or extended,enabling the entire device to change shape as the nested tubeselastically interact. This robot’s kinematic design parametersinclude the pre-curvatures and lengths of each constituent tube.These parameters have a significant impact on the surgicaltargets reachable by the device in constrained spaces, soproper selection of kinematic design parameters for a patient’sanatomy is critical to the success of a medical procedure.

Optimizing a robot’s kinematic design on a task-specificbasis is challenging. We desire to compute high quality designsreasonably quickly (i.e., minutes, not days), particularly formedical applications in which the physician customizes therobot design based on a patient-specific anatomy identifiedin medical images. However, the kinematic design space ofa robot may be large, and for any candidate design wemust evaluate whether that design can move from an initialconfiguration to the goal regions while avoiding obstacles. Thisimplies we need to compute motion plans to multiple goalregions for successively selected designs, but current state-of-the-art motion planners based on sampling-based methodscannot determine with certainty in finite time whether a goalregion can be reached by a particular design.

Our novel contributions are as follows. First, we introducea new method for optimizing the kinematic design parametersof a single piecewise cylindrical robot on a task-specific basisby appropriately integrating sampling-based motion planninginto iterations of a stochastic optimization method for designselection. We implement the integration so that, over time,our reachability evaluations increase in accuracy and ourdesign selections improve toward global optimality. Second,we analyze our algorithm and prove asymptotic optimality,i.e., almost sure convergence to a globally optimal design,which guarantees that our method avoids getting trapped inlocal optima. Third, we demonstrate the broad applicabilityand effectiveness of our design optimization method via evalu-ations for two distinct piecewise cylindrical robots: (i) a 4-linkserial manipulator, and (ii) a concentric tube medical robot.

II. RELATED WORK

Our approach to optimizing the kinematic design of piece-wise cylindrical robots integrates prior work in robot designoptimization, stochastic search, and robot motion planning.

Prior work has investigated combinatorial approaches to de-sign optimization over a finite and discrete set of robot fea-tures. Examples include optimizing over discrete componentsof modular robots [14, 29], monopedal jumping robots [26],snake-like and multi-modal robots [24, 33], and kinematicchains such as proteins [7]. In this paper, we focus on piece-wise cylindrical robots with continuous design parameters.

There has been extensive work on optimizing the kinematicdesign of serial manipulators. Approaches have optimized var-ious metrics and have used genetic algorithms [6, 16, 19, 30],interval analysis [21], geometric methods [32], and grid-based methods [25]. These methods typically lack theoreticalperformance guarantees or achieve computational tractabilityby imposing significant assumptions on the robot’s workspaceand by using simplified kinematic models, which limit theeffectiveness and applicability of the optimization procedure.

Kinematic design optimization for concentric tube robotsis particularly challenging due to their complex kinematics,which is computationally expensive to evaluate due to thecomplex elastic and torsional interactions of their constituenttubes. Morimoto et al. present a complementary approach toautomatic design optimization by providing a human with anintuitive interface to manually design the tubes [23]. Bergeleset al. computationally optimize the robot’s design to reach aset of points without colliding with anatomical obstacles [2].For computational efficiency, they reduce the motion planningproblem to finding goal configurations that can reach thetargets and do not offer a global optimality guarantee. Burgneret al. combine a grid-based evaluation of the robot’s kinematicsin configuration space with a nonlinear optimization methodover the design space to maximize the reachable region ofpoints subject to anatomical constraints [3]. Burgner et al.extended this work to characterize the workspace of concentrictube robots [4, 5]. Ha et al. present a method for generatingdesigns to maximize device stability [12]. By focusing only oncomputing designs and goal configurations, the works abovecannot guarantee that a collision-free path from start to goalexists for the computed design. Torres et al. integrated amotion planner into concentric tube robot design to ensurethe computed design is able to avoid obstacles en route tospecific points [31], but offers slow performance. Baykal et al.investigated computing minimal sets of concentric tube robotdesigns to reach multiple targets [1], although no analysis wasprovided regarding a guarantee on optimality. In this paper, wefocus on the broader class of piecewise cylindrical robots.

III. PROBLEM DEFINITION

A robot’s design d is an n-dimensional vector of kinematicparameters that correspond to physical properties of the robot’sshape that are fixed for the duration of a given task. This vectorincludes kinematic parameters such as the length of each linkof a serial manipulator or the lengths and curvatures of tubesin a concentric tube robot. The design space D ⊂ Rn of arobot is the n-dimensional compact set corresponding to thespace of all possible kinematic designs of the robot.

We assume that the robot operates in a workspace W ⊆ Rkcontaining a compact set of obstacles O ⊂ W , wherek ∈ {2, 3}. The robot should be designed so it can reach(while avoiding obstacles) a set of m user-specified goalregions, where each goal region Gi ⊂ W , i ∈ {1, . . . ,m}is defined as a volume of workspace positions. We defineG = {Gi : i = 1, . . . ,m} as the union of the goal regions.

Let the compact set Q ⊂ Rd denote the d-dimensionalconfiguration space of the robot. Since the shape of a robotat any configuration is a function of its design d, the set ofconfigurations for which the robot’s shape does not intersectan obstacle is dependent on d. Thus, we denote the set ofcollision-free configurations for a robot of design d as Qfree

d ⊂Q. We model the shape of a piecewise cylindrical robot withdesign d ∈ D at configuration q ∈ Q by the mappingShape : D×Q → ([0, 1]→W), which defines the curve of therobot’s backbone, and a radius r of its circular cross-section.Note that Shape(d,q)(0) and Shape(d,q)(1) correspond tothe robot’s base and end-effector points respectively. Weassume Shape is computed using an accurate kinematic model.We define q0 ∈ Q as the robot’s start configuration. Therobot’s motion is a path in the configuration space Q definedby the continuous function σ : [0, 1]→ Q, where σ(0) = q0.A path σ executed under robot design d is collision-free if itlies entirely in the collision-free configuration space, that is,if σ(τ) ∈ Qd

free for all τ ∈ [0, 1].Our objective is to compute an optimal robot design d∗ ∈

D that enables the robot to reach as many goal regions inG as possible in a safe manner, i.e., via collision-free pathsthat avoid the workspace obstacles O. The quality of a designd ∈ D is defined with respect to the extent of the design’sreachability to the goal regions in G. That is, the objectivefunction value of d is expressed as the percentage of goalregions in G that are reachable with design d relative to thetotal number of goal regions in G. Formally, the reachabilityof design d is given by the mapping R(d) : D → [0, 1]:

R(d) :=|GoalRegionsReachable(d)|

|G|, (1)

where GoalRegionsReachable(d) : D → 2|G| denotes the setof goal regions that the robot of design d can reach with its endeffector by following a collision-free path. R(d) expresses thereachable goal percentage of the robot under design d, whichwe seek to maximize. We formalize the objective of kinematicdesign optimization as follows: Given an environment W ⊆Rk (where k = 2 or 3), a set of obstacles O ⊂ W , and aset of user-specified goal regions G, generate a design d∗ thatmaximizes the reachable goal percentage, i.e.,

d∗ ∈ argmaxd∈D

R(d). (2)

IV. METHODS

In this section, we present our algorithm for optimizing thekinematic design parameters of a single piecewise cylindricalrobot to maximize the robot’s reachable goal percentage whileavoiding obstacles in a task-specific environment.

A. Method Overview and the Key Challenge

Our design optimization approach combines a stochasticsearch in the robot’s design space D with a sampling-basedmotion planner in the robot’s configuration space Q to ef-ficiently generate designs with high reachability. To selectcandidate designs for evaluation, we use Adaptive SimulatedAnnealing (ASA) [13], a global optimization algorithm. Foreach selected design, we use the Rapidly-exploring RandomTree (RRT) [17] algorithm to estimate the design’s reachableworkspace and evaluate its reachable goal percentage. Weprovide an overview of our approach in Algs. 1 and 2 andformally prove the method’s asymptotic optimality in Sec. V.

To ensure that we converge toward a globally optimaldesign, a key challenge we must address is that state-of-the-art, practical motion planners cannot guarantee completeness[17], i.e., they cannot always in finite time answer the questionof whether a collision-free motion plan exists from the startconfiguration to a goal region. This limitation of current state-of-the-art motion planners introduces a significant challengefor design optimization; to use a standard optimization algo-rithm to optimize the design d in equation 2, a motion plannermust evaluate the reachable goal percentage accurately andin finite time in each iteration of the optimization algorithm.Commonly used sampling-based motion planners only offerprobabilistic completeness (and in some cases also asymptoticoptimality in terms of path quality), meaning the probabilitythat they will produce a collision-free path (if one exists)to a goal region approaches 1 as more time is spent [17].Terminating a sampling-based motion planner after finite timemay result in an incorrect computation of the reachable goalpercentage. The lack of full completeness makes it impossibleto simply plug a standard sampling-based motion planner intoa standard optimization algorithm and expect convergencetoward a globally optimal design. We address this challengeby appropriately integrating sampling-based motion planninginto stochastic optimization in design space so that, over time,our reachable goal percentage evaluations increase in accuracyand our selected designs approach global optimality.

B. Evaluating a Design’s Reachable Goal Percentage

Evaluating the objective function value R(d) in equation(1) for an arbitrary design d ∈ D requires computingGoalRegionsReachable(d), the set of goal regions that designd can reach by executing collision-free paths. Thus, evaluatingthe reachability of a design is fundamentally a motion planningproblem, which is known to be PSPACE-hard [27]. Thisrenders the use of exact evaluation methods computationallyintractable and motivates the use of a sampling-based motionplanning algorithm, such as the Rapidly-exploring RandomTree (RRT) [17], to generate approximations of a design’sreachability (albeit an approximation that can improve overtime, as will be discussed in Sec. IV-D).

For a given design d ∈ D and a start configuration q0, theRRT algorithm incrementally constructs a tree of configura-tions that can be reached by collision-free paths from the rootof the tree, q0. For a given design, we run the RRT algorithm

Algorithm 1 Select and evaluate a kinematic designInput:

G: set of goal regionsO: set of environmental obstaclesdcurrent: previously considered robot designT : ASA’s current annealing temperaturei: number of RRT iterations to execute

Output:dnew: new robot designRnew: approximate reachable goal percentage of dnew

1: dnew ← SampleDesign(dcurrent, T );2: goalRegionsReached← RRT(dnew, i,O);3: Rnew ← |goalRegionsReached|/|G|;4: return dnew, Rnew;

for i ∈ N+ iterations and iterate over the configurations inthe constructed tree to compute the set of goal regions thatcan be feasibly reached by the robot with design d (Line 2,Alg. 1). From this we can approximate the design’s reachablegoal percentage in a computationally tractable manner (Line3, Alg. 1). Because RRT provides probabilistic completeness,as we increase the iterations i of RRT, the probability of ourapproximation Ri(d) being equal to the true R(d) approaches1. For any finite i, our approximations of the reachable goalpercentage at each iteration is ensured to be a lower bound ofthe ground-truth reachability, i.e., Ri(d) ≤ R(d).

A key challenge is appropriately setting the number ofiterations i the RRT will run for. In Sec. IV-D, we introducean approach to setting i in Alg. 1 that ensures asymptoticoptimality of the design optimization.

C. Selecting Designs

We use the ASA algorithm [13, 20] for optimizing thekinematic design to maximize the reachable goal percentage.We use ASA because it is a global optimization method thatescapes local optima, it is efficient in practice for problemsin high dimensional spaces, and it has favorable algorithmicproperties which we exploit. Specifically, we are able to usesampling-based motion planning in each iteration of ASA ina manner that ensures asymptotic optimality of the design, asdiscussed in Sec. IV-D.

Our ASA-based algorithm is shown as Alg. 2 and operatessimilar to a hill climbing algorithm in that it is centered ona design dcurrent that it incrementally attempts to improve.However, unlike a hill climbing algorithm, the algorithm mayin some iterations select an inferior design, which enables es-caping local minima. Next designs are determined by samplinga new design (SampleDesign; Line 1, Alg. 1) and decidingwhether to accept that new design (Accept; Line 6, Alg. 2),with both procedures being highly dependent on a temperatureparameter T ∈ R≥0. Accept returns true if the sampleddesign d′ is higher quality than dcurrent (i.e., R′ > Rcurrent)or with some probability (dependent on T ) if d′ is inferior.The temperature variable is initially set to a high value and

Algorithm 2 Iterative design optimizationInput:

G: set of goal regionsO: set of environmental obstaclesiinit: initial number of RRT iterationsi∆: additional RRT iterations after each sampledinit(optional): initial design for the search

Output:d∗: a robot design that maximizes (1)

1: i← iinitial; T ← Tinitial; Rcurrent ← 0; R∗ ← 0;2: dcurrent ← random initial design or dinit if provided;3: d∗ ← dcurrent;4: while allotted time remains do5: d′, R′ ← Algorithm1(G,O,dcurrent, T, i)6: if Accept(R′, Rcurrent, T ) then7: dcurrent ← d′;8: Rcurrent ← R′;9: if R′ > R∗ then

10: d∗ ← d′;11: R∗ ← R′;12: i← i+ i∆;13: T ← UpdateTemperature(T );

14: return d∗

is decreased after each iteration based on a cooling schedule(Alg. 2, Line 13). When T is high, ASA is more likely tosample states that are far away from dcurrent and also morelikely to probabilistically accept inferior designs, which leadsto exploratory behavior initially. As T is cooled down overtime, ASA samples states in smaller neighborhoods arounddcurrent and is increasingly less likely to accept inferior designs,which leads to eventual convergence to a high quality design.We cache the best found design (Alg. 2, Line 10) so the bestfound design is returned when the algorithm terminates.

D. Integrating Motion Planning into Stochastic Optimization

A key requirement to converging toward a globally optimaldesign is an accurate evaluation of any candidate design’sreachable goal percentage. This implies we need to computemotion plans to multiple goal regions for each design consid-ered in the optimization, but current state-of-the-art motionplanners based on sampling-based methods cannot specifywith certainty in finite time whether a goal can be reachedby a particular design.

To address this challenge, we use a simple-to-implementidea: we incrementally increase the number of RRT iterationsby i∆ after each sampled design (Line 13, Alg. 2). Thisapproach ensures that our approximations become increasinglyaccurate over time. This approach is sufficient for establishingthe asymptotic optimality of our algorithm (see Sec. V). Thisapproach also has a secondary benefit: it enables us to morequickly (but more coarsely) evaluate many designs in theinitial iterations and subsequently evaluate candidate designswith higher accuracy (albeit at a slower rate) in later iterations.

V. ANALYSIS

We prove under mild assumptions that the design computedby our algorithm almost surely converges [9] to a globallyoptimal design. The outline of our proof is as follows. First,we establish that the set of optimal designs with respect to theproblem in equation 2 has strictly positive measure. Then, weshow that by property of the ASA algorithm, optimal designswill be sampled and evaluated infinitely often. We concludeby proving that, eventually, an optimal design will be sampledand evaluated accurately by the RRT algorithm.

A. Preliminaries

Let γ : Q × Q → R≥0 be the distance function in Q forany arbitrary d ∈ D defined by γ(q,q′) = ||Shape(d,q) −Shape(d,q′)||∞ [17]. For any configuration q ∈ Q, the openball of radius ε centered at q, {q′ ∈ Q | γ(q,q′) < ε},is denoted by Bε(q). A collision-free path under design d,σ : [0, 1]→ Qd

free, is said to have ξ-clearance if

∀q ∈ σ([0, 1]) info∈O||Shape(d,q)− o||∞ ≥ ξ,

where the infimum is taken over all the obstacle points in theworkspace, with a slight abuse of notation in taking the normof a function and the obstacle point o. This analysis focuses onthe piecewise cylindrical robot’s backbone but can extend toa cross-sectional radius r > 0 by appropriately accounting forthe thickness whenever measuring a distance from the robot’sshape to an obstacle in the workspace.

Assumption 1 (Goal Regions as Open Sets). Each goal regionGi ∈ W , i ∈ [m], is defined as an open set.

Assumption 2 (Continuity of the Shape Function). Shape :D×Q → ([0, 1]→W) is continuous over the domain D×Q.

Assumption 1 rules out pathological problem instanceswhere only a single optimal design lying on the boundary ofthe design space exists. Assumption 2 guarantees that robots ofsimilar designs have similar shapes at similar configurations.

B. Sampling Optimal Designs Infinitely Often

Lemma 1 (Paths with Non-zero Clearance). Under any designd ∈ D, any collision-free path σ : [0, 1] → Qd

free has ξ-clearance for some ξ > 0.

Proof: For any d ∈ D and σ : [0, 1] → Qdfree, define

Wσ = {Shape(d,q, s) : q ∈ σ([0, 1]), s ∈ [0, 1]}, i.e., the setof workspace points the robot occupies along the path σ. Notethat Wσ is compact since Shape is continuous on a compactdomain. Since O is also compact and Wσ and O are disjointby definition of collision-free paths, it follows by continuityof norms that 0 < inf {||w − o||∞ : o ∈ O,w ∈ Wσ} = ξ forsome ξ > 0.

Lemma 2 (Sufficient Condition for Reachability). Consider adesign d and path with ξ-clearance σ : [0, 1] → Qd

free, withσ(1) = qgoal ∈ Qd

free. Then, under any design d′ that satisfies

supt∈[0,1]

||Shape(d, σ(t))− Shape(d′, σ(t))||∞ < ξ,

the robot can execute the same path σ to the goal configurationqgoal without colliding with the obstacles.

Proof: By the triangle inequality, for all q ∈ σ([0, 1]):

ξ ≤ info∈O||Shape(d,q)− o||∞

≤ ||Shape(d,q)− Shape(d′,q)||∞+

info∈O||Shape(d′,q)− o||∞

< ξ + info∈O||Shape(d′,q)− o||∞,

which implies that info∈O ||Shape(d′,q)− o||∞ > 0.Let R∗ = maxd∈D R(d) denote the optimal objective value

and let D∗ = {d ∈ D | R(d) = R∗} be the optimalset of designs with respect to the problem in equation 2.The following lemmas establish that designs from the optimaldesign set will be sampled infinitely often.

Lemma 3 (Positive Measure of Optimal Designs). The setof optimal designs, D∗ ⊆ D, has strictly non-zero Lebesguemeasure, i.e., µ(D∗) ∈ R+.

Proof: Consider an arbitrary optimal design d∗ ∈ D thatreaches the set of goal regions G∗ ⊆ G, where R(d∗) =|G∗|/|G| = R∗. By Lemma 1, this implies that for eachgoal region g ∈ G∗, there exists a path with ξg-clearance,σ : [0, 1] → Qd∗

free for some ξg > 0, with σ(1) = qgoal suchthat pg ∈ g, where pg = Shape(d∗,qgoal)(1) denotes theposition of the end-effector at configuration qgoal. Since thegoal region g is an open set (Assumption 1), there exists aconstant εg ∈ R+ such that Bεg (pg) ⊆ g.

Let ε′g = min{εg, ξg} be a strictly positive constant, forsome ξg > 0 as in Lemma 1. By the continuity of the Shapefunction (Assumption 2), we have that Shape is uniformlycontinuous on the compact domain D × Q. Thus, for theconstant ε′g , there exists some δg ∈ R+ such that for anydesign d ∈ D′, it follows that

||Shape(d,q)− Shape(d∗,q)||∞ < ε′g ∀q ∈ σ([0, 1]),

where D′ = {d′ ∈ D | ||d′ − d∗||∞ < δg}.Lemma 2 implies that all designs d ∈ D′ can traverse the

path σ : [0, 1] → Qdfree to reach qgoal without colliding with

the obstacles. Moreover, by definition of the supremum norm,the end-effector of the robot under design d at configurationqgoal is fully contained in goal region g. Thus, we have shownthe existence of a non-empty, open set of designs D′ that canreach goal region g by a continuous, collision-free path.

Following the same line of reasoning, there exist strictlypositive constants εg and ε′g = min{εg, ξg} for each goalregion g ∈ G∗. Since G∗ is a finite set, let ε = ming∈G∗ ε′g bea strictly positive constant. It follows by generalization of theprevious argument for a single goal region that there existsa non-empty, open set of optimal designs, D′′ = {d′′ ∈ D |||d′′ −d∗||∞ < δ}, for some δ ∈ R+, that is, R(d) = R∗ forall designs d ∈ D′′. Lebesgue measure is strictly positive onnon-empty, open sets, thus µ(D′′) ∈ R+. Since D′′ ⊆ D∗ bydefinition, it follows that µ(D∗) ∈ R+.

Lemma 4 (Frequency of Sampling Optimal Designs). Alg. 2will sample designs from the optimal design set D∗ infinitelyoften.

Proof: It is known that designs that are an element ofany subset D′ ⊆ D with non-zero measure will be sampledinfinitely often by the ASA algorithm [13, 20]. Thus, Lemma 3yields the result.

C. Asymptotic Optimality

Let Yk be a random variable that denotes the maximumreachable goal percentage attained over all the designs sampledin optimization iterations 1, . . . , k.

Theorem 5 (Asymptotic Optimality). The solution generatedby Alg. 2 almost surely converges to a globally optimal designd∗ ∈ D∗, i.e.,

P(

limk→∞

Yk = R∗)

= 1.

Proof: Application of Lemma 4 implies that the optimalset of designs D∗ ⊂ D will be sampled infinitely often. Letj ∈ N+ denote the jth occurrence of sampling any arbitraryoptimal design d∗ ∈ D∗ and let Ij denote the number ofiterations that the RRT algorithm is executed for. Note that bythe procedure used to increase the number of RRT iterationsby i∆ ∈ N+ (Alg. 2) after each sampled design, we have thatIj + 1 ≤ Ij+1 for all j and that 1 ≤ I1.

For each occurrence of sampling an optimal design d∗ ∈D, a random approximation of the reachable goal percentageis generated by running the RRT algorithm for Ij iterations.Let Gj(d∗) and Rj(d∗) =

|Gj(d∗)||G| denote the approximation

of GoalRegionsReachable(d∗) and R(d∗) for the jth sampledoptimal design respectively. For any arbitrary ε ∈ R+, let Ajdenote the event |Rj(d∗) − R∗| ≥ ε for each j. Note thatevent Aj is equivalent to the event that the RRT algorithmfails to find a collision-free path to at least one goal regiong ∈ G∗ \ Gj(d∗) within Ij iterations. Thus, we have

P (Aj) = P (∃g ∈ G∗ \ Gj(d∗)) ≤∑g∈G∗

P (g ∈ G∗ \ Gj(d∗))

≤∑g∈G∗

ae−bIj = |G∗|ae−bIj ,

for some constants a, b, where the first inequality is by theunion bound and the second by RRT’s exponential decay of theprobability of failure to find a path after Ij iterations [15, 18].

Consider the sum of the probabilities of Aj over all j:∞∑j=1

P (Aj) ≤∞∑j=1

|G∗|ae−bIj ≤∞∑j=1

|G∗|ae−bj

=|G∗|aeb − 1

<∞.

By the Borel-Cantelli Lemma we have thatP (lim supj→∞Aj) = 0, that is, the probability thatAj occurs infinitely often is 0. This implies that

P (lim infj→∞ |Rj(d∗)−R∗| < ε) = 1, which is preciselythe definition of Rj(d∗)

a.s.→ R∗.Thus, with probability 1, at least one optimal design d∗ ∈

D∗ will be sampled and evaluated accurately as the numberof optimization iterations of Alg. 2 approaches infinity. Sincethe best solution found thus far is cached in Alg. 2, it followsthat Yk

a.s.→ R∗.

VI. RESULTS

We apply our design optimization algorithm to two distinctpiecewise cylindrical robots: (i) a serial manipulator and (ii)a concentric tube robot, a tentacle-like robot designed forminimally invasive medical procedures. We assess the perfor-mance of our method (ASA+MP) in computing designs withhigh reachable goal percentage and compare its computationalefficiency and results with the following variants of our methodand other state-of-the-art design optimization algorithms.• NM+G: The Nelder-Mead optimization algorithm is used

instead of ASA for sampling designs. For evaluation ofthe reachable goal percentage, a grid-based approach isused instead of motion planning; the configuration spaceis discretized into a grid and the robot configuration ateach grid point is evaluated to determine if it is collision-free and reaches a goal region [3].

• NM+MP: Nelder-Mead is used for optimizing the design.In contrast to prior work using Nelder-Mead [3], we usemotion planning using the same number of initial andadditional RRT iterations as our algorithm to approximatethe reachable goal percentage of candidate designs.

• ASA+G: In this simplified form of our approach, we useASA to sample designs, but we use the grid approach (asdescribed in NM+G) to evaluate reachable goal percent-age for a candidate design.

• RRT of RRTs: An RRT-based algorithm is run both inthe design space [31] and in the configuration space forestimating reachable goal percentage.

We emphasize that the grid-based algorithms (ASA+G andNM+G) only consider final configurations when evaluatingreachable goal percentage during design optimization. Thisimplies that grid-based evaluations generate upper bounds onthe ground-truth reachable goal percentage, since the actualmotion of the device from its start configuration to a goalregion is not considered, and no motion may be feasible dueto obstacles. In our results graphs, we do not display thisupper bound; instead, we run a post-processing step (that is notcounted towards method computation time) and estimate thereachable goal percentage of each returned design by runningthe RRT algorithm for 300,000 iterations.

We implemented all design optimization algorithms in C++.The experiments were conducted on a PC with two 2.40 GHzIntel Xeon E5620 processors (8 cores total) and 12 GB RAM.

A. Design Optimization of a 4-link Serial Manipulator

We consider the design optimization of a serial manipulatorwith 4 revolute joints and 4 straight links operating in a 2Denvironment. The configuration space of the robot is defined

Example Instance 1 Example Instance 2 Example Instance 3

Optimaldesign

Genericdesign

Fig. 2. First row: Example configurations of robot designs (where the links are colored green, cyan, magenta, and orange) computed by our algorithm forthree randomly generated 2D environments containing obstacles (red) and a grid of goal regions colored green for grid cells reachable using the optimaldesign and blue for unreachable cells. Second row: In contrast to optimal designs, generic (i.e., randomly generated) robot designs operating in the same threescenarios are unable to reach the goal regions.

0 10 20 30 40 50 60Computation Time (minutes)

0

20

40

60

80

100

Rea

chab

le G

oal P

erce

ntag

e

ASA+MPASA+G

NM+MPNM+G

RRT-of-RRTs

Fig. 3. The performance over time of the design optimization methods for a4-link serial manipulator. The plot shows the reachable goal percentage of thebest design found thus far with respect to computation time, averaged over40 randomized problem instances.

by the angles of the four links, i.e., Q ⊂ (S1)4. We define arobot’s design space as the length of each of the four links,thus, D ⊂ R4. We evaluated each design optimization methodon 40 randomized problem instances. For each instance, werandomly generated between 4 and 8 rectangular or righttriangular obstacles with sides of random length and a setof 100 goal regions arranged in a regular grid and placedrandomly in the workspace. The robot’s start configuration q0

was fixed as 0 for all instances and the robot’s base positionwas randomly placed so that the robot was collision-free.Fig. 2 depicts three examples of the problem instances.

Fig. 3 shows the reachable goal percentage (averaged overthe 40 problem instances) achieved by each design optimiza-tion algorithm as a function of computation time. The robotdesign generated by our algorithm is capable of reaching a

0 10 20 30 40 50 60Computation Time (minutes)

0

20

40

60

80

100

Rea

chab

le G

oal P

erce

ntag

e

ASA+MPASA+G

NM+MPNM+G

RRT-of-RRTs

Fig. 4. Plot of the reachable goal percentage over computation time for eachdesign optimization method run 10 times for the problem instance in Fig. 2(right column).

significantly higher percentage of the goal regions comparedto the designs found by the other algorithms.

Fig. 4 depicts the performance of each design optimizationalgorithm for a single scenario, specifically Example Instance3 in Fig. 2. Each line is an average over 10 runs of thecorresponding algorithm. We note the methods that use grid-based evaluation of the objective function are not guaranteedto improve over time since ignoring motion planning impliesthey are optimizing a potentially incorrect approximation ofthe objective function. Our method improves the design in anasymptotically optimal manner.

Our results indicate that our approach for blending ASA forsearching the design space and motion planning for designevaluation helps in attaining computational efficiency andescapes local optima via asymptotic optimality.

Fig. 5. A concentric tube robot (composed of cyan, yellow, and magentatubes) has the potential to reach clinical goal regions (green and blue voxels)within the lung for early-stage lung cancer diagnosis. The figure shows therobot with an appropriate design reaching a point (green sphere) in one ofthe goal regions (shown as blue).

0 50 100 150 200 250 300 350Computation Time (minutes)

0

20

40

60

80

100

Rea

chab

le G

oal P

erce

ntag

e

ASA+MPASA+G

NM+MPNM+G

RRT-of-RRTs

Fig. 6. The reachable goal percentage over computation time for theconcentric tube robot scenario. The results are averaged across 40 differentproblem instances, each with randomly selected goal regions in the right lung.

B. Design Optimization of a 3-tube Concentric Tube Robot

We next apply our design optimization algorithm to a con-centric tube robot [8, 11], a medical robot composed of nestednitinol tubes that can be rotated and translated independentlyto change the shape of the entire robot and achieve tentacle-like motion. Unlike traditional medical instruments that areconstrained to straight-line paths, these robots are capable ofcurving around anatomical obstacles, e.g., blood vessels, toreach clinical targets in a safe, minimally-invasive manner.

We consider in simulation a concentric tube robot with 3tubes. In configuration space, each tube adds two degrees offreedom (since each tube can be independently inserted androtated), resulting in a 6 dimensional configuration space, i.e.,Q ⊂ (S1)3 × R3. The curvilinear shapes that the robot canachieve are highly dependent on the physical specifications ofthe robot’s tubes, i.e., its design. In this study, each tube of

the concentric tube robot is described by (i) the length of itsstraight section, (ii) the length of its pre-curved section, and(iii) the curvature of its pre-curved section. Thus, for our 3-tube robot the design space is 9 dimensional, i.e., D ⊂ R9.To evaluate the robot’s shape given its configuration, we usean accurate mechanics-based kinematic model to account forthe elastic and torsional interactions between the tubes [28].

Fig. 5 illustrates a potential medical application of thesedevices for biopsy of suspicious nodules in the lung for early-stage lung cancer diagnosis. The concentric tube robot isdeployed near the base of the primary bronchus of the righthuman lung using a rigid bronchoscope with the objectiveof reaching a clinical target for biopsy. We discretized theinterior volume of the right human lung into 4156 equally-sized cubic voxels each with volume ≈ 0.7 cm3. For eachtrial, a subset of 8 contiguous voxels (i.e., goal regions) wasrandomly chosen to represent subregions of a clinical targetthat should be biopsied.

The results averaged over 40 trials are shown in Fig. 6.The results for this scenario follow a similar trend as theresults obtained from the 4-link serial manipulator scenario. Inparticular, the figure illustrates our algorithm’s effectiveness infinding a design with high reachable goal percentage and itstendency to efficiently improve the solution over the allottedcomputation time without being trapped in local optima.

VII. CONCLUSION

We presented a design optimization algorithm applicableto any piecewise cylindrical robot. The algorithm integratesa sampling-based motion planner in the configuration spacewith stochastic search in the design space to efficiently com-pute designs that maximize reachability to user-specified goalregions in the workspace. We proved the asymptotic optimalityof our algorithm and demonstrated its computational efficiencyin simulated scenarios involving serial manipulators and con-centric tube robots for medical procedures.

In future work, we plan to consider a mixture of continuousand discrete design parameters and generalize our definition ofgoal regions to consider goal configurations and end effectorposes. We also plan to physically implement the designscomputed by our method and conduct experiments in testbedsbased on clinically-relevant scenarios, such as lung biopsiesand neurosurgery. Since the shape-set or 3D-printed robotsmay not precisely match our method’s output, we plan toconsider design uncertainty in design optimization. We alsoconjecture that our method and analysis can be extendedbeyond piecewise cylindrical robots.

ACKNOWLEDGMENTS

We thank Robert J. Webster III for valuable discussions,Chris Bowen for his suggestions regarding the analysis, andLuis G. Torres for his insights into design optimization andsoftware for evaluating designs. This research was supportedin part by the National Science Foundation under award IIS-1149965 and by the National Institutes of Health under awardsR01EB017467 and R21EB017952.

REFERENCES

[1] Cenk Baykal, Luis G Torres, and Ron Alterovitz. Op-timizing design parameters for sets of concentric tuberobots using sampling-based motion planning. In Proc.IEEE/RSJ Int. Conf. on Intelligent Robots and Systems(IROS), pages 4381–4387, 2015.

[2] Christos Bergeles, Andrew H Gosline, Nikolay V Vasi-lyev, Patrick Codd, Pedro J del Nido, and Pierre E.Dupont. Concentric tube robot design and optimizationbased on task and anatomical constraints. IEEE Trans.Robotics, 31(1):67–84, 2015.

[3] Jessica Burgner, Hunter B Gilbert, and Robert J. WebsterIII. On the computational design of concentric tuberobots: Incorporating volume-based objectives. In Proc.IEEE Int. Conf. Robotics and Automation (ICRA), pages1185–1190, 2013.

[4] Jessica Burgner-Kahrs. Task-specific design of tubularcontinuum robots for surgical applications. In SoftRobotics, pages 222–230. Springer, 2015.

[5] Jessica Burgner-Kahrs, Hunter B Gilbert, JosephineGranna, Philip J Swaney, and Robert J. Webster III.Workspace characterization for concentric tube contin-uum robots. In Proc. IEEE/RSJ Int. Conf. IntelligentRobots and Systems (IROS), pages 1269–1275, 2014.

[6] Olivier Chocron. Evolutionary design of modular roboticarms. Robotica, 2008.

[7] Laurent Denarie, Kevin Molloy, Marc Vaisset, ThierrySimeon, and Juan Cortes. Combining system design andpath planning. In Proc. Workshop on the AlgorithmicFoundations of Robotics (WAFR), 2016.

[8] Pierre E. Dupont, Jesse Lock, Brandon Itkowitz, andEvan Butler. Design and control of concentric-tuberobots. IEEE Trans. Robotics, 26(2):209–225, 2010.

[9] Rick Durrett. Probability: theory and examples. Cam-bridge university press, 2010.

[10] Hunter B. Gilbert and Robert J. Webster III. Rapid, reli-able shape setting of superelastic nitinol for prototypingrobots. IEEE Robotics and Automation Letters, 1(1):98–105, 2016.

[11] Hunter B. Gilbert, D. Caleb Rucker, and Robert J.Webster III. Concentric tube robots: The state of the artand future directions. In Int. Symp. Robotics Research(ISRR), 2013.

[12] Junhyoung Ha, Frank C Park, and Pierre E Dupont.Achieving elastic stability of concentric tube robotsthrough optimization of tube precurvature. In Proc.IEEE/RSJ Int. Conf. Intelligent Robots and Systems(IROS), pages 864–870, 2014.

[13] Lester Ingber. Very fast simulated re-annealing. Mathe-matical and Computer Modelling, 12(8):967–973, 1989.

[14] Gangyuan Jing, Tarik Tosun, Mark Yim, and HadasKress-Gazit. An end-to-end system for accomplishingtasks with modular robots. In Proc. Robotics: Scienceand Systems (RSS), 2016.

[15] Sertac Karaman and Emilio Frazzoli. Sampling-based

algorithms for optimal motion planning. Int. J. RoboticsResearch, 30(7):846–894, 2011.

[16] Lalitesh K. Katragadda. A Language and Framework forRobotic Design. PhD thesis, Carnegie Mellon University,1997.

[17] Steven M. LaValle. Planning Algorithms. CambridgeUniversity Press, Cambridge, U.K., 2006.

[18] Steven M LaValle and James J Kuffner. Randomizedkinodynamic planning. Int. J. Robotics Research, 20(5):378–400, 2001.

[19] Chris Leger. Automated Synthesis and Optimization ofRobot Configurations: An Evolutionary Approach. PhDthesis, Carnegie Mellon University, 1999.

[20] Marco Locatelli. Simulated annealing algorithms forcontinuous global optimization. In Handbook of GlobalOptimization, pages 179–229. Springer, 2002.

[21] Jean-Pierre Merlet. Optimal design of robots. In Proc.Robotics: Science and Systems (RSS), 2005.

[22] Tania K Morimoto and Allison M Okamura. Design of3-d printed concentric tube robots. IEEE Trans. Robotics,32(6):1419–1430, 2016.

[23] Tania K Morimoto, Joseph D Greer, Michael H Hsieh,and Allison M Okamura. Surgeon design interface forpatient-specific concentric tube robots. In Proc. IEEEInt. Conf. on Biomedical Robotics and Biomechatronics(BioRob), pages 41–48, 2016.

[24] Yash Mulgaonkar, Brandon Araki, Je-sung Koh, LuisGuerrero-Bonilla, Daniel M Aukes, Anurag Makineni,Michael T Tolley, Daniela Rus, Robert J Wood, and VijayKumar. The flying monkey: a mesoscale robot that canrun, fly, and grasp. In Proc. IEEE Int. Conf. Roboticsand Automation (ICRA), pages 4672–4679, 2016.

[25] J. Park, P. Chang, and J. Yang. Task-oriented design ofrobot kinematics using the grid method. Int. J. AdvancedRobotics, 17(9):879–907, 2003.

[26] Mark M Plecnik, Duncan W Haldane, Justin K Yim,and Ronald S Fearing. Design exploration and kinematictuning of a power modulating jumping monopod. Journalof Mechanisms and Robotics, 9(1):011009, 2017.

[27] John H. Reif. Complexity of the mover’s problemand generalizations. In 20th Annual IEEE Symp. onFoundations of Computer Science, pages 421–427, 1979.

[28] Daniel Caleb Rucker. The mechanics of continuumrobots: model-based sensing and control. PhD thesis,Vanderbilt University, 2011.

[29] Daniela Rus and Michael T Tolley. Design, fabricationand control of soft robots. Nature, 521(7553):467–475,2015.

[30] Damien Salle, Philippe Bidaud, and Guillaume Morel.Optimal design of high dexterity modular MIS instru-ment for coronary artery bypass grafting. In Proc. IEEEInt. Conf. Robotics and Automation (ICRA), pages 1276–1281, 2004.

[31] Luis G Torres, Robert J. Webster III, and Ron Alterovitz.Task-oriented design of concentric tube robots usingmechanics-based models. In Proc. IEEE/RSJ Int. Conf.

Intelligent Robots and Systems (IROS), pages 4449–4455,2012.

[32] R. Vijaykumar, K. J. Waldron, and M. J. Tsai. Geometricoptimization of serial chain manipulator structures forworking volume and dexterity. Int. J. Robotics Research,5(2):91–103, 1986.

[33] Cornell Wright, Austin Buchan, Ben Brown, Jason Geist,Michael Schwerin, David Rollinson, Matthew Tesch, andHowie Choset. Design and architecture of the unifiedmodular snake robot. In Proc. IEEE Int. Conf. Roboticsand Automation (ICRA), pages 4347–4354, 2012.