Cable Route Planning in Complex Environments Using Constrained Sampling

Ilknur Kabul Russell Gayle Ming C. Lin

Department of Computer Science, University of North Carolina at Chapel Hill, USA

{ilknurk,rgayle,lin}@cs.unc.edu
http://gamma.cs.unc.edu/CABLE PLAN

Abstract tion and motion planning. Cable and wire routing can be posed as
a robot motion planning problem to compute the path for a highly
articulated and deformable robot, i.e. the cable (or the wire). How-
We present a route planning algorithm for cable and wire layouts
ever, the dimensionality of the configuration space of such a de-
in complex environments. Our algorithm precomputes a global
formable robot is very high. In addition, the simulation of highly
roadmap of the environment by using a variant of the probabilistic
articulated and deformable bodies can be expensive and time con-
roadmap method (PRM) and performs constrained sampling near
suming.
the contact space. Given the initial and the final configurations,
we compute an approximate path using the initial roadmap gener-
ated on the contact space. We refine the approximate path by per-
forming constrained sampling and use adaptive forward dynamics
to compute a penetration-free path. Our algorithm takes into ac-
count geometric constraints like non-penetration and physical con- 1.1 Main Results
straints like multi-body dynamics and joint limits. We highlight the
performance of our planner on different scenarios of varying com-
plexity. In this paper, we present a novel approach for cable routing in com-
plex environments. We assume that the cable can be modeled as
Keywords: motion planning, adaptive dynamics, articulated bod- a highly articulated robot with multiple links and many degrees of
ies, cable route planning freedom (dofs).

1 Introduction Our route planning algorithm is based on a variant of PRM that

samples near the surfaces of the obstacles in the workspace. It ini-
tially generates random samples at the corners and edges of the en-
Designing cable and wire layouts is often a complex and tedious
vironment. If a path cannot be found, it samples on the obstacles by
process in building construction. Incorrect cable and wire layouts
using a ray shooting approach. The resulting global roadmap com-
can be costly due to poor planning and may require significant mod-
puted this way lies near the C-obstacles in the configuration space,
ifications and design reviews. Current practices include scaled pro-
also known as the contact space [Latombe 1991]. This roadmap is
totypes onto which the cable (or wire) layout is constructed by us-
then used as the guiding path to plan the motion of the entire ca-
ing route planning algorithms for motionless rigid cable segments.
ble. During the simulation phase, whenever a collision occurs be-
CAD systems are often used to assist the process during the early
tween the first link of the cable and the environment, the initial path
design stage. In these systems, the paths of the cables or wires are
is adjusted based on constrained sampling, which recomputes a
first calculated, then rigid segments are put through these paths to
new node near the contact space. Our method expands the initial
simulate cable or wire layout. However, current CAD systems do
roadmap from a node belonging to the contact space (computed
not take into account the cables or wires dynamics properties and
from a contact point) and finds a new node around the neighbor-
their interaction with the environment. It should be noted that in
hood of the contacting node using the projection method [Wilhelm-
some applications dynamic, or even quasi-static, simulation may
sen 1976]. In order to simulate cable dynamics, we use an adaptive
not be necessary. But, while actually laying a cable or simulating
forward dynamics algorithm [Redon et al. 2005] with quasi-static
the process, forces such as friction or gravity may influence the path
numerical integration that selects the most important joints to per-
taken to arrive at the goal configuration. For instance, in pulling or
form bounded error dynamic simulation in a hierarchical manner.
pushing a long cable toward a goal configuration, the friction of the
The quasi-static integration removes unnecessary oscillation due to
cable with a passage or corner may influence what path is taken.
inertia which helps provide smoother robot motion. Moreover, we
Or, if an interactive visualization for rapid prototyping is required,
develop efficient collision handling techniques to resolve the con-
it would be important to model the bending or sagging of the cable
tacts for the remaining links during the simulation.
between supports due to gravity. In these cases, dynamics is nec-
essary to generate physically plausible paths, configurations, and
motion. Our algorithm achieves realistic simulation results and performs
each simulation step at interactive rates. We have implemented and
To the best of our knowledge, there is very little or no work done on tested our system on Alienware AMD AthlonT M 64 X2 Dual-Core
cable (or wire) routing that includes both realistic physical simula- with 2.41 GHz Processor 4800++ and 2GB of RAM. Our algorithm
can simulate a path for cables consisting of 200-300 links (or dofs)
at interactive rates (averaging about 17 ms per simulation step) in
modestly complex environments with tens of thousands of poly-
gons. Our adaptive dynamics algorithm provides significant per-
formance gain compared to the prior linear-time forward dynamics
algorithms. In addition, our constrained sampling technique gen-
erates effective and useful samples near the obstacles, such as the
walls, doorways, and around other support structures.
1.2 Overview 2.3 Motion Planning for Articulated and De-
formable Robots
The rest of the paper is organized as follows. In Section 2, we
briefly survey the prior work on motion planning for deformable Motion planning is a well studied problem in robotics. Most of the
robots, cable simulation and cable routing. We give an overview work has been on rigid or articulated robots with a few degrees of
of our approach in Section 3. We present our cable simulation freedom. Our variant of PRM and constrained sampling is similar
algorithm based on adaptive forward dynamics in Section 4. We to the OBPRM algorithm [Amato et al. 1998] and local planning
describe our planning algorithm in Section 5 and highlight its per- in the contact space [Redon and Lin 2005], although our realiza-
formance in Section 6. tion is quite different. In addition, we incorporate physical and me-
chanical constraints using adaptive forward dynamics and contact
handling, whereas prior algorithm mostly deal with geometric con-
2 Previous Work straints such as non-penetration.
In this section, we give a brief overview of prior work in cable sim- There is relatively less work on motion planning for deformable
ulation, cable routing and motion planning for deformable objects. objects. Some of the earlier work on deformable robots included
specialized algorithms for bending pipes [Sun et al. 1996], cables
[Nakagaki and Kitagaki 1997] and metal sheets [Ngugen and Mills
2.1 Cable Simulation 1996]. Holleman et al. [Holleman et al. 1998] present a proba-
bilistic planner capable of finding paths for a flexible surface patch,
Cable simulation has been studied in the recent years. Hergenrother modeled as a low degree B`ezier patch, using an approximate energy
and Dahne present an algorithm for the real-time simulation of vir- function to model deformation of the part. Guibas et al. [Guibas
tual cables [Hergenrother and Dahne 2000]. Their simulation is et al. 1999] describe a probabilistic algorithm for a surface patch,
based on inverse kinematics. They model the cable by using con- modeled as the medial axis of the workspace. Anshelevich et al.
secutive cylinder segments that are connected by ball joints. Given [Anshelevich et al. 2000] present a path planning algorithm for
the start and goal positions of the cable, their algorithm calculates simple volumes such as pipes and cables by using a mass-spring
the shape of the cable by considering energy minimization. In prac- representation. Lamiraux et al. [Lamiraux and Kavraki 2001] pro-
tice, their algorithm is fast and applicable to interactive applica- pose a probabilistic planner capable of finding paths for a flexible
tions. However, their approach does not simulate the dynamics of object under manipulation constraints. The objects deformation is
the cable and does not perform collision handling, which is essen- computed by using the principles of elastic energy from mechanics
tial for realistic applications. which makes the motion planning difficult for handling the end con-
straints and finding minimum energy curves. In [Moll and Kavraki
Loock and Schomer describe an application of rigid body simu- 2004], a different curve parametrization technique is used for han-
lation to assembly tasks in virtual environments and extend their dling low-energy configurations. In addition, contact points with
system to real-time simulation of deformable cables [Loock and simple obstacles are considered in finding a minimal energy curve
Schomer 2001]. They model the cable as a chain of rigid seg- configuration. However, finding the exact contact points that makes
ments. The cable simulation is achieved by using mass-spring the curve at minimum energy is still a difficult task. Bayazit et al.
model with generalized springs. They use stiff linear springs and [Bayazit et al. 2002] describe a two-stage approach that initially
torsion springs to preserve the length of the cable. computes an approximate path and then refines the path by apply-
Gregoire and Schomer present a numerically stable and physically ing geometric-based free-form deformation to the robot. Gayle et
accurate simulation tool for one dimensional components in [Gr- al. [Gayle et al. 2005] present an algorithm for path planning for
goire and Schomer 2006]. They model the bending and torsion us- a flexible robot in complex environments. The algorithm computes
ing the Cosserat model and they use a generalized spring-mass sys- collision free paths based on physical and geometric constraints.
tem with a mixed coordinate system for the simulation. However, The collision detection between deformable object and the environ-
their simulation is slow for modeling long cables. For example, the ment is accelerated by using graphics processors. Saha et al.[Saha
simulation time for cable that has 300 points is around 128 ms per and Isto 2006] present a motion planning technique for the manipu-
step. lation of deformable linear objects. The application of their method
in self-knotting and knotting around simple static objects by using
coordinating dual robot arms is illustrated in the paper. The mo-
2.2 Cable Routing tion planning algorithm depends on the geometrical model of the
deformable object and the robot arms. It does not consider any
The design of a cable harness is a complex and costly process. physical properties of the deformable linear object. Many of these
Many approaches have been proposed to automate the design pro- algorithms exploit geometric properties and often do not consider
cess. Conru [Conru 1994] describe a system to route a cable har- the physical constraints of the robot, such as collision detection and
ness using genetic algorithms. The genetic algorithms are used to contact handling. Some recent approaches deal with general de-
search for routes which are close to the global minimum. The cable formable robots and environments [Rodriguez et al. 2006], but do
harness routing problem is decomposed into generating a harness not model friction and motion constraints (e.g. joint limits) which
configuration and computing a route for the harness in the environ- could be necessary to realistically model the interaction between
ment. Both of these problems are solved using genetic algorithms. the cable and the rest of the environment.
Holt et al.[Holt et al. 2004] present a virtual reality system to aid
cable harness designers. Their approach focuses on usage of human
engineers knowledge in the design process. Their goal is to provide
3 Overview
an engineer with interactive tools to design a human-in-the-loop
system. In this section, we give an overview of our planning algorithm.
We introduce the notation used in the remainder of the paper and
Both of these approaches do not consider the physical properties of present our framework to solve motion planning as a constrained
the cable. They assume that the cable is composed of rigid segments dynamical system and model the robot as an articulated chain with
and the number of segments can be adjusted by the user. a high number of dofs.
3.1 Notation 4 Cable Simulation
We assume that each cable can be modeled as a sequence of m rigid The geometric and physical characteristics of the cables should be
bodies connected by m 1 2-dof revolute joints. The joints are simulated realistically and efficiently for real-time applications. In
implemented as two 1-dof revolute joints, one of which is rotated this section, we first state our assumptions and the basic represen-
90 degrees about the central axis of the cable. The configuration, tation for modeling cables, then we describe our approach for sim-
C(t), of the cable at time t can be described as a vector of joint ulating cable dynamics.
angles along with the position and orientation of the base of the
robot. The position of the head (first link) of the cable is represented
by Chead (t), which is the position and orientation of this link.
4.1 Assumptions and Representations
We assume that the set of obstacles are rigid and they are repre- Cables have various material properties depending on its type and
sented as O = {o1 , o2 , ...} in the workspace, W . A roadmap, G = usage. In this work, we assume that a cable conserves its length dur-
{V, E}, in the free workspace, the space external to the obstacles, ing the simulation. A cable consists of rigid segments that cannot
consists of a milestones V = {v1 , v2 , ...} and links E = {e1 , e2 , ...}. be stretched or sheared. Its length remains constant when dragged
A path in this roadmap is an acyclic sequence that connects two or pulled. Its cross section is undeformable and the mass of each
milestones. segment is same along the entire cable. To model each cable, the
Problem Formulation: The problem can be stated as follows: Find length of the cable can be specified by the number of rigid segments
a sequential set of cable configurations C(t1 ),...,C(t f ) such that no or the length of each rigid segment. By changing one of these pa-
C(ti ) intersects with any obstacle in O, and C(ti ) is near the obsta- rameters, the user can model cables of varying lengths. Based on
cles, where C(t1 ) and C(t f ) are the initial and final configurations these assumptions, we can model each cable as a chain of articu-
of the robot (cable) respectively. lated linkages, i.e. as a highly articulated robot.
The optimal algorithm for computing the dynamics of a kinematic
chain takes linear time [Redon et al. 2005]. To approximate a cable
3.2 Cable Simulation well using a highly articulated chain would require many linkages.
This computation can become rather costly for long cables. There-
When simulating a cable, there are a number of considerations fore, we use the adaptive forward dynamics algorithm [Redon et al.
which must be taken into account. First, to model a realistic cable, 2005] for cable simulation. The main advantage of this approach is
we assume that its length will not change during the simulation. that it lazily recomputes the forward dynamics of a cable by only
This holds since little or no stretching will occur in most wires and simulating the joints that best approximate the overall motion of
cables when forces are applied to the ends. the entire chain with bounded errors. The adaptive dynamics au-
tomatically selects active joints, or those which will be simulated,
In addition, our simulation must preserve geometric and mechan-
based on motion error metrics to compute an error-bounded approx-
ical properties of the cable itself. This accounts for penetrations
imation of the articulated-body dynamics. The user can change the
with the environment and also with itself as well as preventing a
motion error metric for controlling the number of joints that will be
cable from bending too much. To efficiently simulate the cable, we
active or rigidified during the simulation.
take advantage of adaptive forward dynamics for articulated bodies
[Redon et al. 2005] and develop a new collision handling method. The contact handling is also an important issue in cable simulation.
Since many constraints can introduce unwanted oscillation in the The cable should deform realistically due to contacts and the pen-
robot motion due to large penalty response forces, we use a quasi- etration should be prevented when there is a collision between the
static variation of Euler integration in order to advance the simula- cable and the nearby obstacles. We develop a penalty-based contact
tion. response capable of handling joint limits and other external forces
along with a fast collision detection method to achieve sub-linear-
Our planner applies several constraints to the cable in order to en-
time collision handling for multi-body systems.
sure that it can successfully reach its goal. Path constraints are used
to move it along a specified path, and collision constraints ensure Next we will describe the basic components of the adaptive multi-
that it is penetration free and also enforces joint angles. body dynamics and contact handling for our cable simulation.

3.3 Motion Planning 4.2 Articulated-body dynamics

Our planning algorithm builds upon constraint-based motion plan- The adaptive dynamics algorithm is built upon Featherstones
ning (CBMP) proposed by [Garber and Lin 2002]. divide-and-conquer algorithm (DCA) [Featherstone 1999a],
[Featherstone 1999b]. Featherstones algorithm is a linear time
This approach involves two main stages; a roadmap generation algorithm to compute the forward dynamics of an articulated body
stage and an execution stage. The first stage is largely done as a based on the forces applied to it. The algorithm relies on the
precomputation step and is responsible for finding a guiding path following articulated-body equation:
through the environment. This guiding path does not need to be
completely collision free with respect to the robot. The second


a1
1 12 1m f1 b1
stage uses constrained dynamical simulation to move the robot
a2 21 2 2m
f2
b
2

along this guiding path toward the goal. This allows the robot to . = . .. .. .. .. + .

. . . ..
,
locally adapt the guiding path to its own structure. . . . . .

am m1 m2 m
fm m
b
There are several advantages to the CBMP approach. Guiding path
(1)
generation is both efficient and simple, while the path itself also
automatically ensures that both geometric and physical constraints where ai is the 6 1 spatial acceleration of link i, fi is the 6 1
i is the 6 1 bias acceleration of link
spatial force applied to link i, b
are preserved. For our problem, this allows the cable to move along
a simple guiding path without violating its constraints. i (the acceleration link i would have if all link forces were zero), i
 algorithm:
AABB hierarchies update: For each mobile rigid link, we
determine a bounding AABB using the root OBB of the OBB-
Trees [Gottschalk et al. 1996]. We then compute an AABB for
each node of the assembly tree (i.e. for each subassembly of
the articulated body) using a bottom-up pass. We thus obtain
one AABB hierarchy per articulated body, whose structure is
identical to the assembly tree of the articulated body.
AABB culling: Using the AABB hierarchies, we detect po-
tentially colliding rigid links and objects.
OBB culling: When two rigid objects are found to potentially
intersect after the AABB culling step, we recursively and si-
Figure 1: Construction of an articulated body. An articulated body multaneously traverse their OBB hierarchies to help local-
A is connected to body B at the principal joint, j2 , to form body ize potential collisions between pairs of triangles [Gottschalk
C. The assembly tree for C is shown beneath the body. Forces and et al. 1996].
accelerations which govern Cs motion are shown.
Triangle/triangle intersection tests: Whenever two leaf-
OBBs are found to overlap, a triangle/triangle intersection test
is the 6 6 inverse articulated-body inertia of link i, and i j is the is performed to determine whether the triangles contained in
6 6 cross-coupling inverse inertia between links i and j. the leaf-OBBs intersect. When they do, we report the corre-
sponding intersection segment.
The DCA employs a recursive definition of an articulated body: an
articulated body is a pair of articulated bodies connected by a joint. After collisions between the cable and the obstacles have been de-
The forward dynamics of the articulated body are computed in es- termined, the collision response is computed using a penalty-based
sentially two steps: main pass and back-substitution pass. When approach as described in Sec. 4.5.2.
the DCA completes, all joint accelerations and all kinematic con-
straint forces are known.
4.5 Constraint Forces
In order to advance the state of the simulation, a quasi-static vari-
ation of Euler integration is used. In this variation, the body is In our simulation, along with internal forces for kinematic con-
considered to be at rest throughout the integration step. Thus, both straints, we apply external constraint forces for making the cable
angular and translational velocities are zero and integration only move along the computed path and for handling interaction between
considers the net acceleration. the cable and the obstacles. Next, we will describe the formulations
of these constraint forces.

4.3 Adaptive articulated-body dynamics

4.5.1 Attraction Force to the Goal
Featherstones DCA is linear in the number of joints in the articu-
lated body. Determining a path or resolving contacts for a highly To make the rest of the cable follow the computed path, we apply
articulated body could be prohibitively slow using a typical forward attraction forces to the first link of the cable along the direction of
dynamics algorithm. In order to improve the performance of the the path. This force is basically a spring force between the first link
planner, we incorporate the adaptive dynamics algorithm by Redon of the cable and the computed path.
et al. [Redon et al. 2005] to lazily simulate the articulated body
motion that best represents the overall motion of the robot with
an error-bounded approximation. Essentially, this enhanced algo- fiAttraction = k(Li Lrest ) (2)
rithm allows us to systematically choose the appropriate number of
joints that are simulated in the articulated body, by automatically In this formulation, k is the spring constant, Li is the distance be-
determining which joints should be simulated, in order to provide a tween the path and the first link of the cable, and Lrest is the intended
high-quality approximation of the articulated-body motion. distance between the first link of the cable and the path.

4.4 Collision Handling 4.5.2 Contact Handling Forces

The collision detection between the cable and the environment is When a collision is detected between the cable and the obstacle, we
performed using a hybrid bounding volume hierarchy. We perform apply a force in the direction of contact normal to keep the cable
two culling steps, based on axis-aligned bounding boxes (AABBs) apart from the surface of the obstacle. Let xi be the position of
and oriented bounding boxes (OBBs), to help localize potential col- contact link i, xipro j is the projection of xi on the obstacle and Ni
lisions, before performing intersection tests at the triangle level. We is the normal vector at position xipro j . Then the collision response
pre-compute and store one hierarchy of oriented bounding boxes for force will be proportional to the penetration distance d, where
each rigid cable link and each rigid environment obstacle. We also
precompute one axis-aligned bounding box for each obstacle in the d = (xipro j xi )Ni (3)
environment. The OBB hierarchies and AABBs of environment
obstacles do not have to be updated during planning.
and the contact handling force will be
At runtime, we determine the intersections between the cable and
the environment obstacles using the following collision detection fiContact = kNi d (4)
where k is the collision coefficient. line segment between vi and v j that does not intersect with any ob-
stacle in O. For connecting the initial and final configuration to the
Given the basic steps of our cable simulation module, we will next roadmap, we also place them in the milestone set and perform local
describe our motion planning algorithm and show how our cable planning for them as well.
simulation is used in our cable route planning.

5.2 Computing a Guiding Path

5 Motion Planning for Cable Layouts
Once we have generated a roadmap that provides the desired
Our cable route planning algorithm is composed of three key amount of coverage, we use it to generate paths between some given
phases: start and goal configurations. We first link the start and goal con-
figurations to the roadmap. In particular, we add the start and goal
1. Global Roadmap Generation configurations to our graph as query nodes and create edges be-
2. Guiding Path Estimation tween these nodes and all other nodes reachable from them. After
these nodes have been added we can execute any graph search to
3. Path Adjustment and Constrained Dynamics Simulation find a path from the start to the goal. We use Dijkstras shortest
path algorithm with distance values as weights for computing the
(a) If there exists a collision between the first link and the initial path.
environment, compute the next milestone using con-
strained sampling and modify the path accordingly;
(b) For the remaining links, simulate the cable motion using 5.3 Simulation and Runtime Path Modification
adaptive forward dynamics with efficient contact han-
dling and positional constraints. Once an estimated path is computed, we begin our simulation loop
with the robot in its initial configuration. To make the cable follow
Next, we will describe each step in more detail. the path during the simulation, we apply our attraction force to the
first link of the cable in the direction of the path. This makes the
first link of the cable move along the direction of the path, however
5.1 Global Roadmap construction the rest of the cable does not always follow the approximate path
due to the dynamics of the cable. In order to constrain the motion
The probabilistic roadmap (PRM) algorithm is commonly used for of the entire cable, we apply additional path constraint forces to a
robot motion planning. We develop a variant of PRM for computing subset of the links of the cable which are selected based on their
the roadmap of the environment. For simplicity and efficiency, we position with respect to the curved regions of the path segments
first treat the cable as a point robot and then make path adjustments followed by the first link of the cable. This approach is similar to
using constrained sampling and constrained dynamics simulation. fixing portions of the cable against a wall to ensure it remains there.
This strategy makes the roadmap construction fast and effective. As the cable traverses the path, we check the collisions between the
cable and the obstacles. If there exists a collision between the cable
and the obstacles, we apply collision handling algorithm to avoid
5.1.1 Contact-Space Sampling the penetrations. In addition, if there is a collision between the first
segment of the cable and the obstacles, we update the current path
One of the important aspects of any randomized roadmap planner segment by computing a new, constrained milestone as described in
is the method used to generate samples in the configuration space. Sec. 5.4.
Since typically cables and wires should be placed in such a way that
they attract as little attention as possible, one of our sampling goals For each simulation or path-query step, we apply the following al-
is to find samples in the free space which are close to the obstacles gorithm:
(e.g. on or near the wall) and near the corners of the buildings.
1. Apply attraction force (Sec. 4.5.1) to the first link of the cable;
We achieve this goal by systematically finding corners of a large
structure. This is done by first computing the intersections of three 2. Sum all forces and deform the cable using adaptive forward
or more constraint planes (walls), then the intersections of two con- dynamics (Sec. 4.3);
straint planes. Random samples are first taken at these locations. 3. Perform collision detection (Sec. 4.4) between cable and ob-
These points are shifted away from the walls by an amount to stacles;
ensure that the local planning can construct the roadmap of the en-
vironment. After generating the samples near the corners, we con- 4. Apply contact response (Sec. 4.5.2) to resolve the collisions;
struct our roadmap and we check whether a valid path exists. If
5. If there exists a collision between the first segment of the cable
there is no viable path from the initial to the goal configuration,
and the obstacle, update the current path segment between v i
then extra milestones are generated by shooting rays on these con-
and vi+1 .
straint planes (walls), which typically are the xy-, yz-, or xz planes
for a building. (a) Compute new milestone vnew using constrained sam-
pling (the approach is presented in section 5.4);

5.1.2 Local Planning (b) Update the position of milestone vi+1 to vnew ;
(c) Check collision between the new path segment vi -vi+1
Another important step in PRM is local planning which connects and the environment.
collision-free nodes. We use a distance-based approach for finding
the edges in the roadmap from a milestone vi . All milestones that We repeat steps (1) - (4) until a path is found between the initial and
are within some search radius of vi are placed into a neighbor set, final configurations, vi and v f . If no path is found, it is reported and
N. For each milestone v j in N, we determine if there is a straight the user can either add more milestones or conclude that there is no
safe path. Otherwise, we end the simulation when it is reported that House Model-This model consists of over 14,500 polygons
the object is sufficiently close to the final configuration. and the cable is represented as a 280-link robot with 5,000
polygons. The house has four rooms and there is only one
entry to the house. The house does not have a ceiling.
5.4 Constrained sampling
Building Model-This model consists of over 18,200 polygons
Given the contact information between the first segment of the cable and the cable is represented as a 280-link robot with 5,000
and the obstacle, we can determine the local tangent space of the polygons. The building has seven floors and each floor has
obstacles for a given pair of sampled nodes near the obstacles. We different shapes and dimensions. In order to illustrate the
use this tangent space to constrain the search of a new node when cable simulation and planning visually, we place the cable
we found an intersection between the first link of the cable. and the around the outside of the building.
obstacles.
Car Model-This model consists of over 19,668 polygons and
Assume vi is a point on the path and an external force toward this the cable is represented as a 280-link robot with 5,000 poly-
point is applied to the robot. Let P = (Px , Py , Pz )T denote the Carte- gons. This model is obtained from the MPK model database.
sian coordinates of the contact point in the world frame. The contact
point P corresponds to a point on the first link of the robot, and is Our algorithm runs at interactive rates, averaging about 60 fps. Ta-
a 3-vector P(Ci ) depending on the configuration Ci of the cable and ble. 1 illustrates the timings of our planning and sampling technique
the environment. Let n = (nx , ny , nz )T denote the Cartesian coordi- in detail.
nates of the contact normal in the world frame, and assume that this We also benchmark our algorithm by changing the sampling tech-
normal is directed toward the exterior of the obstacle. The local nique in our simulation. Instead of constrained sampling, we use
non-penetration constraint is: random sampling in 3D. In constrained based sampling, we gen-
erated samples near the corners, edges and walls, and we applied
dP(Ci ) n 0, (5) constraint sampling during the simulation. However, in random
where dP(Ci ) is the small variation in the position of P correspond- sampling the samples are randomly generated in 3D. It is important
ing to a small variation d(Ci ) around the configuration Ci of the to note that each sampling step takes longer but ensure no colli-
robot. This non-penetration constraint defines a polyhedral set of sion between samples using our constrained sampling. When using
valid variations. Provided the robot in the configuration Ci is in a random sampling, each step takes much less time, but many sam-
consistent state with no interpenetration, the valid variations set is ples generated are not necessarily in free space, thus can result in
non-empty. longer simulation time to compute a collision free path. In this
Because we use linear constraints, the set of valid variations is only benchmarking experiment, we use our adaptive dynamics simula-
a local piecewise-linear characterization of the valid space around tion framework and collision handling method in both cases for ca-
vi . This set of valid variations can be efficiently used for local plan- ble simulations. Table. 2 shows the timing comparison of two sam-
ning. At runtime, whenever there exist a contact between the first pling techniques. In all cases, the precomputatin using constrained
link of the robot and the obstacle, we use the set of valid variations sampling takes roughly same amount of time as random sampling.
to help determine a new node vnew , by projecting the intentional But, the simulation time is less using roadmaps computed from our
variation on the set of valid variations. More specifically, whenever constrained sampling method.
a new tentative node vt is randomly chosen in the neighborhood of To demonstrate the path quality generated by our algorithm with
the contact-space node, we perform the following operations before constrained sampling in the contact space vs. using random sam-
checking its validity with a discrete collision checker: pling for only the first segment of the cable in our approach, we
1. Compute the corresponding tentative variation dvt = vt vi . show a comparison image highlighting the difference in path quality
for the House in Fig. 2. Notice that constrained sampling results in
2. Project the tentative variation dvt on the set of valid variations better cable placement than random sampling for cable route plan-
to obtain the new variation dv. ning. The computed path using constrained sampling tend to stay
around the corners, the edges, and walls as real cables should; while
3. Set the new milestone vnew = vi + dv
the computed path using random sampling may be a valid one but
The projection of the intentional variation dvt onto the polyhedral can be obtrusive or unsafe for human operating in the same work
set of constraints is performed by using the Wilhelmsen projection space.
algorithm [Wilhelmsen 1976].
It is also important to note that we cannot apply standard PRM di-
Finally, we note that the combination of constrained sampling in the rectly as the planner must know the final configurations of all cable
contact space and sampling by ray shooting allows us to accumulate segments, in order to find the intermediate configuratons for the en-
obstacle constraints: whenever a new node vnew is determined by tire cable, which has a very high degrees of freedom and can take
extending a node vi , we check whether the near-obstacle constraints a very long time to plan any path. The final configuration of the
of vi , are still valid in the new configuration vnew . This occurs for entire cable is what we also aim to compute at the same time in this
example when the robot comes in contact with an obstacle. paper. And, the final configuration of all cable segments can be au-
tomatically computed and simulated using our integrated approach
presented here.
6 Results
We also include additional sequences taken from our simulation
In this section, we present the results of our approach on three dif- runs (see Fig. 3). Our cable route planning algorithm is able to
ferent benchmarks of varying complexity. Below are our bench- automatically compute realistic paths for these modestly complex
mark scenarios: models. Our algorithm using constrained sampling systematically
finds milestones which are either on or near to the obstacles (walls,
Bridge Model- This model consists of over 5,000 polygons corners, structural supports, etc.) and the cable moves along the
and the cable is represented as a 280-link robot with 5,000 path naturally using our adaptive multi-body dynamics at interac-
polygons. tive rates. Please refer to the project website,
 Scene # of Tri. in Cable # of Tri. in Environment Total joints Motion Error Metric Avg. Time Dynamics (s) Avg. Time Simulation (s) Total Sim. Time (s)
Bridge 5,600 5,000 280 0.01 % 0.0026 0.0045 27.03
0.1 % 0.0019 0.0040 23.67
0.5 % 0.0013 0.0034 20.07
House 5,600 14,500 280 0.01 % 0.0030 0.0051 49.98
0.1 % 0.0022 0.0048 47.04
0.5 % 0.0017 0.0041 40.01
Building 5,600 18,200 280 0.01 % 0.0031 0.0051 41.31
0.1 % 0.0025 0.0047 38.07
0.5 % 0.0019 0.0040 32.40
Car 5,600 19,668 280 0.01 % 0.0034 0.0055 32.49
0.1 % 0.0020 0.0040 23.66
0.5 % 0.0016 0.0037 22.23

Table 1: Performance for different benchmarks

Constrained Based Sampling Random Sampling

Scene Total joints Motion Error Metric Roadmap+Query Time Total Simulation Time Roadmap+Query Time Total Simulation Time
Bridge 280 0.5 % 12.31 20.07 8.82 26.82
House 280 0.5 % 36.42 40.01 53.26 49.86
Building 280 0.5 % 41.64 32.40 39.74 42.93
Car 280 0.5 % 46.21 22.23 43.25 26.23

Table 2: Comparison between Constrained Based Sampling (CBS) and Random Sampling

http://gamma.cs.unc.edu/CABLE PLAN Acknowledgement

for more path sequences generated by our cable route planner. The authors would like to thank our reviewers for the useful feed-
back which helped to improve the work. This work was supported
in part by a Department of Energy High-Performance Computer
Science Fellowship administered by the Krell Institute and ARO
Contracts W911NF-04-1-0088, NSF awards 0400134, 0429583
7 Conclusion and 0404088, DARPA/RDECOM Contract N61339-04-C-0043 and
Disruptive Technology Office.
We present a novel algorithm for planning of cable layouts in com-
plex environments. We propose a variant of PRM using a novel con-
strained sampling coupled with a fast adaptive forward dynamics al- References
gorithm and efficient collision handling. Our approach is applicable
A MATO , N., BAYAZIT, O., DALE , L., J ONES , C., AND VALLEJO , D. 1998. Obprm:
to many applications, such as wire routing. We have demonstrated An obstacle-based prm for 3d workspaces. Proceedings of WAFR98, 197204.
our algorithm on four interesting scenarios and the initial results are
A NSHELEVICH , E., OWENS , S., L AMIRAUX , F., AND K AVRAKI , L. 2000. De-
rather promising.
formable volumes in path planning applications. ICRA.

Our current approach has some limitations. We first perform con- BAYAZIT, O. B., L IEN , J., AND A MATO , N. M. 2002. Probabilistic roadmap motion
planning for deformable objects. ICRA, 21262133.
strained sampling for the head of the cable and then the rest of the
cable follows by adaptively computing its forward dynamics with C ONRU , A. B. 1994. A genetic approach to the cable harness routing problem. In
contact handling for the entire cable. This algorithm does not guar- Proceedings of the IEEE World Congress on Computation Intelligence.
antee physically-accurate motion at all times, but an error-bounded F EATHERSTONE , R. 1999. A divide-and-conquer articulated body algorithm for par-
approximation. Nevertheless, the resulting path is always kinemat- allel o(log(n)) calculation of rigid body dynamics. part 1: Basic algorithm. Inter-
ically valid and collision free. national Journal of Robotics Research, 18(9):867875.
F EATHERSTONE , R. 1999. A divide-and-conquer articulated body algorithm for paral-
There are several directions for future investigation. It is possible lel o(log(n)) calculation of rigid body dynamics. part 2: Trees, loops, and accuracy.
that we can tightly integrate the adaptive forward dynamics frame- International Journal of Robotics Research, 18(9):876892.
work with constrained sampling. This may result in a more robust G ARBER , M., AND L IN , M. 2002. Constraint-based motion planning for virtual
system, but at higher computational costs. We can apply continuous prototyping. UNC Technical Report.
collision detection to the cable simulation during the path follow- G AYLE , R., W.S EGARS , L IN , M., AND M ANOCHA , D. 2005. Path planning for
ing. deformable robots in complex environments. In Robotics: Systems and Science.
G OTTSCHALK , S., L IN , M., AND M ANOCHA , D. 1996. OBB-Tree: A hierarchical
We would also like to explore cable route planning for cables with structure for rapid interference detection. Proc. of ACM Siggraph96, 171180.
multiple branches, which can be useful in some real-world appli- G RGOIRE , M., AND S CHOMER , E. 2006. Interactive simulation of one-dimensional
cations. In addition, we plan to apply our algorithm to more com- flexible parts. ACM Symposium on Solid and Physical Modeling, SPM06, 95103.
plex environments, such as a power plant. We would like to further G UIBAS , L., H OLLEMAN , C., AND K AVRAKI , L. 1999. A probabilistic roadmap
improve the roadmap construction and collision detection for mas- planner for flexible objects with a workspace medial-axis-based sampling ap-
sive models consisting of many millions of geometric primitives. proach. In Proc. of IROS.
And, we would like to apply our simulation in a wider variety of H ERGENROTHER , E., AND DAHNE , P. 2000. Real-time virtual cables based on kine-
situations, such as bundled cables, adding supports for a single or matic simulation. In Proceedings of the WSCG.
multiple cables, and also to include a wider variety of constraints.
 Figure 3: Cable route planning on the (a) Car Model (b) Bridge Model (c) Building Model (d) House Model

NAKAGAKI , H., AND K ITAGAKI , K. 1997. Study of deformation tasks of a flexible

wire. Proc. of IEEE Int. Conf. on Robotics and Automation.
N GUGEN , W., AND M ILLS , J. 1996. Multi-robot control for flexible fixtureless as-
sembly of flexible sheet metal auto parts. Proc. of IEEE Int. Conf. on Robotics and
Automation, 23402345.
R EDON , S., AND L IN , M. 2005. Practical local planning in the contact space. ICRA.
R EDON , S., G ALOPPO , N., AND L IN , M. 2005. Adaptive dynamics of articulated
bodies. ACM Trans. on Graphics (Proc. of ACM SIGGRAPH) 24, 3.
RODRIGUEZ , S., L IEN , J., AND A MATO , N. 2006. Planning motion in completely
deformable environments. Proc. of IEEE Int. Conf. Robot. Autom. (ICRA).
S AHA , M., AND I STO , P. 2006. Motion planning for robotic manipulation of de-
formable linear objects. ICRA.
S UN , D., S HI , X., AND L IU , Y. 1996. Modeling and cooperation of two-arm robotic
system manipulating a deformable object. Proc. of IEEE Int. Conf. on Robotics and
Automation, 23462351.
W ILHELMSEN , D. R. 1976. A nearest point algorithm for convex polyhedral cones
and applications to positive linear approximations. Mathematics of computation,
4857.

Figure 2: Path quality comparison on the House Model using our

algorithm with constrained sampling in the contact space (Top) and
with random sampling (Bottom). Notice that the cable is better
placed near the support structures on the walls with constrained
sampling. Using random sampling, the cable is simply routed from
its initial configuration to the final configuration through unclut-
tered space and the placement of the cable appears rather awkward.

H OLLEMAN , C., K AVRAKI , L., AND WARREN , J. 1998. Planning paths for a flexible
surface patch. IEEE Int. Conf. Robot. Autom. (ICRA).
H OLT, P., R ITCHIE , J., DAY, P., S IMMONS , J., ROBINSON , G., RUSSELL , G., AND
N G , F. 2004. Immersive virtual reality in cable and pipe routing: Design metaphors
and cognitive ergonomics. In Journal of Computing and Information Science in
Engineering, vol. 4, 161277.
L AMIRAUX , F., AND K AVRAKI , L. 2001. Path planning for elastic objects under
manipulation constraints. International Journal of Robotics Research, 20(3):188
208.
L ATOMBE , J. 1991. Robot Motion Planning. Kluwer Academic Publishers.
L OOCK , A., AND S CHOMER , E. 2001. Virtual environment for interactive assembly
simulation: From rigid bodies to deformable cables. In Multiconference on Sys-
temics, Cybernetics and Informatics (SCI01), Virtual Engineering and Emergent
Computing.
M OLL , M., AND K AVRAKI , L. 2004. Path planning for minimal energy curves of con-
stant length. Proceedings of International Conference on Robotics and Automation.