Content uploaded by Julius Sustarevas

Author content

All content in this area was uploaded by Julius Sustarevas on Jul 11, 2022

Content may be subject to copyright.

Hybrid Discrete-Continuous Path Planning for Lattice Traversal

Santiago Franco*1, Julius Sustarevas*2, Sara Bernardini1,

Abstract— Lattice structures allow robotic systems to operate

in complex and hazardous environments, e.g. construction,

mining and nuclear plants, reliably and effectively. However,

current navigation systems for these structures are neither

realistic, as they assume simplistic motion primitives and

obstacle-free workspaces, nor efﬁcient as they rely solely on

global discrete search in an attempt to leverage the modular-

ity of lattices. This paper tackles this gap and studies how

robots can navigate lattice structures efﬁciently. We present a

realistic application environment where robots have to avoid

obstacles and the structure itself to reach target locations.

Our solution couples discrete optimal search, using a domain-

dependent heuristic, and sampling-based motion planning to

ﬁnd feasible trajectories in the discrete search space and in the

continuous joint space at the same time. We provide two search

graph formulations and a path planning approach. Simulation

experiments, based on structures and robots created for the

Innovate UK Connect-R project, examine scalability to large

grid spaces while maintaining performances close to optimal.

I. INTRODUCTION

Weight efﬁciency, high stiffness, and strength of lattice-

like cellular structures have made them the focus of an

increasing body of research in the ﬁelds of self-assembly

and autonomous construction [1]–[4]. As illustrated in Fig. 1,

often, in these applications, the system design includes a

robot that needs to traverse a lattice to undertake assembly

or inspection tasks. To do that, the robot must ﬁnd a feasible

path from one part of the structure to another, giving rise to a

unique path planning problem. While the modular nature of

the structure lends itself to discrete search, motion planning

is carried out in a continuous joint space. In this paper, we

propose a novel methodology that brings these two aspects

together, satisfying the main assumptions of discrete search

and leveraging the kinematic capabilities of modern robotics.

A signiﬁcant work that explores robotic assembly of lattice

structures is the project TERMES [5]. Inspired by termites, it

features a team of small robots building a rectilinear structure

from modular magnetic blocks. The robots can pick up the

blocks, climb an existing structure and attach new blocks

according to a building plan. The research explores assembly

sequencing while maintaining structure traversability at any

point during construction. A similar project by Jenett et al.

[6], [7] propose a Material-Robot system that leverages a

complex multi Degree of Freedom (DoF) robot assembling

cub-octahedral and octahedron lattice elements. Here, multi-

ple bipedal robots traverse and construct the structure simul-

taneously. However, both of these works adopt a simpliﬁed

primitives-based approach for robot motion. In the case of

∗Both authors contributed equally to this work.

1{Santiago.FrancoAixela},{Sara.Bernardini}@rhul.ac.uk, Royal Hol-

loway University of London. 2info@ross-robotics.co.uk, Ross Robotics Ltd

Fig. 1: Lattice Structure Traversal Problem: Lattice components are

shown in grey, obstacles in white and connectors (sockets) in green.

Robot starting connector marked in yellow, end connector in red.

TERMES, the robot relies on mechanical solutions to assure

motion success and can only move orthogonally, climbing

up or down. The Material-Robot system uses only a handful

of human prescribed and pre-computed primitive motions.

While use of primitives simpliﬁes the path ﬁnding problem

and allows the use of standard discrete search algorithms

such as A∗, it fails to leverage the capabilities of multi-DoF

robot kinematics and requires strong assumptions such as

an obstacle free environment and a completely rigid lattice

structure. These assumptions compromise the applicability

of self-assembly structures to real-world applications.

This paper is inspired by recent work on self-assembling

structures for nuclear decommissioning [8]. In this setting,

information about the environment and structure is available,

but obstacles might be present. Furthermore, as exposure to

radiation is undesirable, minimizing the time the robot takes

to traverse the structure becomes crucial. The lattices in this

paper are based on the structural robot prototype discussed

in [8]. Therefore, we propose a methodology on how joint

space robot motion planning and discrete lattice search can

be coupled, allowing complex collision-aware motions. The

contributions of this paper are as follows: 1) we propose

two graph formulations for path ﬁnding in lattice structures,

requiring different computational efforts; 2) we propose a

discrete planning approach for these graphs; and 3) we carry

out an extensive evaluation of our approach in simulation.

The structure of this paper is as follows. Section II reviews

literature on self assembly systems, climbing robots and

discrete search. In Sec. III, we describe how the lattice, the

robot and the planning problem is modelled. We then propose

graph formation in Section IV and search approaches in

Section V. Section VI offers an evaluation of our techniques

in simulation and discusses their qualitative and quantitative

properties. Conclusions are presented in Section VII.

II. REL ATED WORK

1) Climbing Robots and Lattice Traversal: Together with

self-assembly and construction, systems similar to ours have

been used for inspection [9]–[11] and maintenance [12]. A

common type of robot considered for these problems is the

Inchworm archetype [13]. These robots consist of a single

kinematic chain that is symmetrical about a revolute joint.

The two ends of the kinematic chain are used to attach the

robot to the environment creating a biped-like motion.

Considering structure traversal, leveraging structure dis-

creteness could help ﬁnd paths over it using established

search methodology. The Material-Robot system [6], for

example, after deﬁning motion primitives, uses A∗with the

Manhattan heuristic. However, further work by Jenett et al.

[14] examines compliance and deformation within the lattice

structures and thus highlights the necessity to plan online.

When motion primitives are not assumed, ﬁnding optimal

intermediate states or gait sequences for the inchworm robot

becomes a challenge. A work by Yang et al. [15] describes

how a graph can be constructed by considering the end-

effector landing locations as the base for future steps. To ﬁnd

a feasible path, they use Breadth First Search and validate

each edge via an online call to motion planning. Determining

these gaits or attachment locations is also explored by Zhu

et al. [16], who focus on biped robots climbing over truss-

like structures. The robot’s kinematic capabilities determine

its ability to perform a collision free transition from one

truss to another, in turn informing global path planning. The

search space is small as only a few trusses are considered,

however determining the grip locations as part of the problem

increases its complexity. The related problem of climbing

tree branches is studied by Lam et al. [17], [18]. The robot

used in these works has a ﬂexible continuum body, but

locomotion-wise is similar to the inchworm archetype. These

works shows how tree branches can be described as a graph

and locations along the branches are discretized to allow

for dynamic programming search. However, the problem

is simpliﬁed by preventing the robot switching between

branches, which reduces the graph’s size.

Pagano et al. [19] proposes a more complex scenario

where incomplete information about the world is assumed.

In this work, an inchworm robot uses depth sensors to allow

an iterative Line of Sight Tree approach to ﬁnd intermediate

robot states. Meanwhile, a potential ﬁeld motion planning is

used to ﬁnd the joint space motion that the robot needs to

follow. While this approach runs online and is highly ﬂexible,

it is far from optimal as it requires the robot to backtrack

frequently to gather additional information.

2) Discrete Search: The robotics works discussed above

tend to split the discrete and continuous parts of the problem

and apply different methods to each part. Divide and conquer

approaches are also seen in discrete search literature, e.g. the

TIM planner [20], where a generic planner uses a specialized

planner to solve a subset of the problem. Semantic attach-

ments follow a similar strategy [21], [22].

In this paper, we run A* multiple times as we learn the real

cost of actions. LPA* [23] is an incremental type of A* that

repeatedly calculates an plan from a ﬁxed initial state to the

goal state, when some of the action costs change. The ﬁrst

search is identical to A*, following searches place nodes in a

priority queue for reevaluation if a node becomes inconsistent

due to changes to the predecessor edge’s costs. Expansions

are ordered by f-value and tie-breaked by min(g,rhs), where

rhs is a look-ahead value based on the minimum gvalues

of the node’s predecessors. LPA* is guaranteed to expand

every state at most twice per search episode. LPA* is not

guaranteed to be correct (always ﬁnd optimal plan) when

using admissible but inconsistent heuristics, unlike A*. In

our application, the A* search effort is insigniﬁcant for most

problems, however for the largest discrete search spaces

LPA* should be considered.

D* [24] can be used to generate optimal paths in a partially

known environment. In our case, the discrete environment is

fully known, but edges need to be validated because obsta-

cles or deformations require ad hoc solutions. We ignore

obstacles in the discrete search space because validating all

expanded edge costs would be too expensive.

III. PROB LEM MODELLING

In this section, we outline how the lattice structure and the

robot are modeled and state the path planning problem we

tackle. We also describe the software framework we used.

A. The Lattice Structure

To capture the construction and self-assembly use cases

presented in the literature [6], [8], we generalize the lattice

structure as a 3-dimensional grid of homogeneous cubes of

side d(d=0.35m in our experiments). Fig. 1 shows how

cubes form an overall lattice with obstacle cubes in white.

Each exposed face of each lattice cube has a connection

socket that a robot can attach itself onto. Both lattice and

obstacle cubes have an associated collision box, but there is

a gap (1cm) between the socket frame and the collision box

to allow easy motion planning when attaching to the socket.

A socket coordinate can be represented as a vector of

integers s= (x,y,z,f), where x,y,z∈Z,f∈ {0,1,2,3,4,5}.

The x,y,z components translate into multiples of cube widths.

The face value fdenotes the index of cube faces expressed

as the rotations {I,Ry(π

2),Rx(−π

2),Ry(−π

2),Rx(π

2),Ry(π)},

where Iis the identity matrix and Rx,Ryare rotation matrices

about the x and y axes. Fig. 2 illustrates the socket naming

and frame orientation conventions.

Fig. 2: Socket frame x,y,z axes shown in red, green and blue.

The conventions above allow a compact description of

socket coordinate in grid-world. The homogeneous transfor-

mation matrix denoting the socket frame can be found as the

following multiplication of translations and rotations:

Ts=Trans(d·(x,y,z))Rot(Rf)T rans(d

2·(0,0,1)) (1)

B. The Multi-Task Bot

Following literature, we adopt the inchworm robot

archetype. For our experiments we simulated the inchworm

type robot created for the Connect-R project [25]. We refer to

the robot as Multi-Task Bot (MTB). We model the MTB as a

symmetrically arranged 5-DOF arm. The robot is illustrated

in Fig. 3. In our experiments, we use a robot with link lengths

of d1=11.015 cm, d2=5.065 cm, d3=33.6 cm. Robot

joints 2-4 rotate about their y axes, while joints 1 and 5

rotate about z axis. Rotation limits for joints 1-5 are ±180◦,

±95◦,±121◦,±95◦,±180◦, respectively. We also assume all

joints have the same velocity limit, set to ±0.5 rads−1and

gear reduction ratio of 90.

Fig. 3: Connector frames and joints of the MTB robot. Joints 1 and

5 rotate about the z-axis, while joints 2-4 rotate about the y-axis.

The robot is designed to be symmetrical about joint 3,

making the ends of the kinematic chain interchangeable.

This also means that we only require one kinematic model

to describe the robot both ways along the chain. For the

robot to ﬁx itself to the structure, a connector frame must

fully overlap a lattice socket frame, orientation included. As

a convention, we say root connector to mean the one that

remains ﬁxed during robot motion. Given a homogeneous

transform to a root connector TRand a joint state j=

(j1,j2,j3,j4,j5), the leaf connector frame can be found via

Forward Kinematics TL=FK (TR,j). To model the reverse,

we deﬁne a robot joint inverse function as follows:

inverse(j) = (−j5,−j4,j3,−j2,−j1),then

if TL=FK(TR,j),it follows:

TR=FK(TL,inverse(j))

(2)

C. Software Framework

The lattice and robot described in this section are modeled

using tools from the Robot Operating System (ROS) ecosys-

tem. All transformation calculations are carried out using

the ROS tf and PyKDL frameworks. Robot motion planning

uses the MoveIt! [26] software package, where the lattice

structure populates the planning scene of the robot. The robot

inverse kinematics are computed via the Trac-ik solver [27].

We use two joint-space planners, RRTConnect [28] and

RRTStar [29], as they are implemented by the Open Motion

Planning Library (OMPL) [30]. Both planners are sampling-

based and adopt the Rapidly exploring Random Tree (RRT)

methodology. The RRTStar planner is asymptotically optimal

and we set its optimization objective as to minimize the

maximum element of joint distance traveled.

D. Path Planning Problem Statement

Consider the problem in Fig. 1. A multi-task robot is

attached to the lattice structure at a starting socket s0and

its joint conﬁguration is known. The position and size of the

lattice cubes, the obstacles and the set Sof all free sockets

on the lattice are also assumed to be known. The robot is

tasked with connecting to any free socket on a goal cube

g= (gx,gy,gz)located on the lattice, i.e. the set of goal

sockets is Sg={(gx,gy,gz,0...5)} ⊂ S. The planner must

ﬁnd a sequence of sockets siand corresponding joint space

trajectories Ji,i+1taking the robot from s0to one of the goal

state sg∈Sgoal , such that each Ji,i+1is feasible and collision

free.

IV. GRA PH FORMATION

In this section we tackle the lattice structure path planning

problem. We ﬁrst examine the problem statement presented

in the previous section and propose two alternatives of

how lattice and robot descriptions can lead to a discrete

search graph construction. The vertices of these graphs will

correspond to robot state, while edges - to robot motion.

Then, in Section V, we show that calling continuous joint

space planners to retrieve graph edge costs during discrete

search leads to poor performance and scalability. To address

this, we describe how computing an ofﬂine cache can provide

asymptotically-minimal estimated edge costs and in doing so

allow an iterative search-validation approach.

A. Single Socket ab-Graph

A naive way to form a search graph is by choosing on

the neutral joint conﬁguration that the MTB takes when

attached to a single socket. We use the all-zero conﬁguration

j=0, corresponding to the MTB fully stretching out (see

Figure 4a). In this way, tuple v= (s,0)corresponds to

placing the MTB root connector at Tsand setting all joints

to zero. This tuple describes the MTB state completely and,

in turn, deﬁnes a graph vertex v. An edge of such a graph

corresponds to the MTB hopping from one socket to another,

starting and ending with j=0, see Fig. 4. Only vertices with

mutually reachable sockets share an edge, this is discussed in

Sec. IV-C. Computing the joint space trajectory for an edge

between sockets aand bis thus composed of four steps:

1) Find the inverse kinematics solution j∗=IK(Ta,Tb).

2) Compute the collision aware trajectory J1taking the MTB

from state (a,0)to (a,j∗).

3) Compute the collision aware trajectory J2taking the MTB

from state (b,inverse(j∗)) to (b,0).

4) Concatenate the trajectories: Ja,b= [J1,J2].

We call this graph formulation the Single Socket ab-graph,

signifying that each edge corresponds to two sockets, while

vertices to one. Note that the joint inverse function allows us

to use a single robot kinematics model during computation.

For Ja,bto be performed on real hardware, the J2component

needs to be inverted back.

The ab-graph’s main advantage is that each vertex in the

graph can be described by using a single socket. However,

two expensive joint-space motion plans must be formulated

to compute the edge costs. Also, as the neutral state has

the same conﬁguration for all sockets no matter of their

surroundings, there might be cases in which the MTB

ends up colliding with nearby obstacles, invalidating the

corresponding vertices.

B. Double Socket abc-Graph

A graph can also be built by using two sockets for a

vertex and three for an edge (see Figure 4b). We call this

graph Double Socket abc-graph, signifying three sockets per

edge where socket bis common to both the initial and

ﬁnal vertex. Here, a vertex is a tuple v= (a,b,ja,b), s.t.

ja,b=IK(Ta,Tb). The pair of sockets a,bmust be mutually

reachable. Assuming that, for each socket s∈S, we have

access to the set of reachable sockets Cs⊂S, the pairs then

are (a,b),∀a∈S,∀b∈Ca. We explain in the next section

how the set Csis obtained. Note that there can be multiple or

even inﬁnite (in case of high-DoF robots) inverse kinematics

solutions between two sockets. For this reason, when we

create the graph data structure, we ﬁnd a single ja,bof each

vertex. A path planning algorithm operating on this graph

will not consider alternative ja,band as such, solutions found

or optimality properties are restricted to the speciﬁc set of

ja,bsassociated with the vertices. Furthermore, the vertices

(a,b,ja,b)and (b,a,inverse(ja,b)) correspond to the exact

same robot state. To avoid unnecessary vertices, we store

unique vertices only by using alphanumerical sorting.

The abc-graph dominates the ab-graph in terms of com-

pleteness. The ab-graph requires to fully extend the MTB

after connecting to a new socket while the abc-graph only

requires that a physical connection is possible between two

sockets. Hence, there are multiple cases where a socket

is reachable in the abc-graph while unreachable in the

ab-graph. Any solution in the ab-graph has at least one

equivalent solution in the abc-graph while the opposite is not

necessarily true. This is critical when dealing with obstacles.

Any particular set of reachable sockets Cscan be obtained

by appropriately translating and rotating sockets inside C0.

In our implementation, we compute the homogeneous trans-

formations for all sockets in the CM, perform the frame

transformation Tsand cast the transforms back into socket

tuples. The resulting set is then intersected with the set of

free sockets Son a lattice, obtaining Cs⊂S.

Finally, an edge of the abc-graph corresponds to a single

hopping motion between any two reachable vertices that

share a socket, with the robot start and end joint conﬁgura-

tions being the ones associated with the vertices, see Fig. 4.

The joint space trajectory for the motion can be computed

as follows:

1) Find the matching socket bbetween (a,b)and (b,c)vertices.

2) Appropriately invert the joint conﬁgurations associated to

(a,b)and (b,c)vertices to obtain jb,aand jb,c.

3) Compute the collision aware trajectory J(a,b),(b,c)taking the

robot from state (b,jb,a)to (b,jb,c)

(a) ab vertex and edge (b) abc vertex and edge

Fig. 4: Robot motions for ab and abc graphs. Final vertex positions

are shown in full opacity, intermediate conﬁgurations are transpar-

ent, and the edge motions are show by the arrows.

C. Connectivity Map

The above descriptions on how the ab and abc graphs

can be constructed rely on knowing a set of reachable

sockets Cs⊂Sto which the robot can connect while still

being attached to socket s∈S. To ﬁnd these sockets, we

compute the robot’s Connectivity Map (CM) C0empirically.

We create a fully populated grid world of size ncentered

at world origin. Grid size is chosen chose so it encapsulates

robot reach volume. In our case, n=7, i.e. all cubes between

(−3,−3,−3)to (3,3,3)exist. All six faces of all cubes

in the grid have sockets. Collision detection for this setup

is disabled. We attach the robot to socket (0,0,0,0)and

compute the inverse kinematics IK(T0,Ts∈Sgrid )to all sockets

in this grid. The sockets for which an IK solution is found

form the robot connectivity map C0. In our case, the CM is

comprised of 53 sockets.

Any particular set of reachable sockets Cscan be obtained

by appropriately translating and rotating sockets inside C0.

In our implementation, we compute the homogeneous trans-

formations for all sockets in the CM, perform the frame

transformation Tsand cast the transforms back into socket

tuples. The resulting set is then intersected with the set of

free sockets Son a lattice, obtaining Cs⊂S.

D. Graph Validation

For both types of graphs, a collision check is performed so

only valid vertices are considered. The Connectivity Map is

used to determine the existence of the graph edges. For single

socket ab-graphs, the edges exist for all vertices v,w= (s,0)

such that ws∈Cvs. For double socket, abc-graphs, edges

exist for all vertices v,w= (a,b,ja,b)such that waor wb

is in Cva∪Cvb. Note that this check also guarantees that v

and wshare a socket. Finally, we deﬁne the edge cost as

the time taken by the collision free edge motion, which is

dependent on the structure and obstacles. Thus, at the graph

construction stage, edge costs are left unset.

V. PATH PLANNING FOR TRAVER SAL O F LATTICES

In Section IV, we describe how the ab and abc graphs can

be constructed to capture the lattice structure and a robot’s

ability to traverse it. We can now deﬁne a global search

problem that captures the path planning problem stated in

Sec. III-D. Given the set of free sockets S, the goal cube

gthat deﬁnes the goal sockets Sgand a starting socket

s0, the global search problem can be deﬁned as the tuple

Π=hV,v0,Vg,Ji, where Vis the set of the graph vertices

constructed from S,v0is the initial vertex corresponding to

s0,Vg⊂Vis a set of goal vertices such that the robot is

attached to any of the goal sockets in Sgand Jrepresents all

the available actions, i.e. the joint trajectories that a robot can

perform. A plan π=J0,J1,..., Jnis made up of consecutive

actions that take the robot from v0to a state vg∈Vg. A

plan is optimal if there is no alternative plan reaching any

vg∈Vgwith a smaller cost. The function c(J)represents the

cost of each action, that is the time of the joint trajectory

J. The search problem seeks to minimize c(π) = ∑J∈πc(J).

To distinguish between the two types of graphs, we use

v= (s,0)∈Vab and v= (a,b,ja,b)∈Vabc.

A. A∗with Online Motion Planning

To grasp an intuition on the search problem, we ﬁrst

examine how a naive A∗approach performs on both graphs.

The graph connectivity is computed ofﬂine, but every time

A∗attempts to obtain the edge cost of a new edge, the

RRTConnect joint space planner is called to ﬁnd a joint

trajectory Jtaking the robot from one vertex state to the

other as described in Sections IV-A and IV-B. The RRT-

Connect planner is fast but not optimal and is used only

for illustration. Using an optimal planner for this test would

lead to very poor performance (we obtained no meaningful

data in our experiments). Also, efﬁcient A∗planning requires

an informed admissible heuristic. Here, we use a Weighted

Euclidean heuristic. We describe and prove the admissibility

of this heuristic in an upcoming section. The results of testing

A∗with online motion planning are shown in Fig. 5.

Fig. 5: Number of Solved and OutOfTime (2 h) instances over 100

trials with online validation. Lattice sizes range from 6 to 21.

Figure 5 shows that online edge cost validation scales up

poorly, even without obstacles, as the lattice size increases.

Section VI shows how a cache-based approach scales better.

Note that using greedy search algorithms would generally

reduce the number of node expansions and hence speed

up ﬁnding solutions. However, we next present an iterative

method which preserves optimality while greatly reducing

average node expansion costs. If optimality is not required,

the proposed iterative approach could easily be adapted to

greedy search methods as well.

B. Iterative A∗With Cached Costs

Figure 5 shows that calculating a joint space solution for

each node expansion is impractical even in the best case

scenario (without any obstacles). Instead, we propose an

iterative A∗with cached costs approach. Firstly, we compute

an ofﬂine cache of asymptotically optimal costs associated

with the ab and abc motions. This cache can then be used

to obtain admissible and consistent cost estimates during

A∗vertex expansion. We also use these costs to derive the

weights for the Weighted Euclidean heuristic. To ensure the

paths are feasible, A∗will be called iteratively, performing

path validation between iterations.

1) Costs Cache Computation: We compute the cache by

performing multiple joint space plans for ab and abc motions

using the RRTStar planner. We use the same grid setup as

in Section IV-C, disable collisions with obstacles, but keep

MTB self collisions. Vertex pairs considered for ab cache

are shown in Eq. 3, for abc in Eq. 4:

((0,0,0,f),b)∀f∈ {0..5},∀b∈C0(3)

((0,0,0,f),a),((0,0,0,f),b)∀f∈ {0..5},∀a,b∈C0(4)

We do not assume a single IK solution for ((0,0,0,f),a)

and ((0,0,0,f),b). IK is called every time, this way many

IK solutions are tried and the approach is applicable to

higher DoF robots. Furthermore, as described in Sec. III,

cube sockets have speciﬁc orientations. Therefore, the whole

cache data structure cannot be computed only for f=0. For

example, I·Ry(−π

2)6=Rx(π

2)·Ry(−π

2). This does not affect

the CM computation, but when planning a motion, joints

could travel a different distance and so all sockets on cube

(0,0,0)are tested. Structure-dependent symmetries could be

exploited to speed up cache computations. A total of 100

trials are computed for each edge, in order to capture all

possible IK solutions, and the fastest is taken. The resulting

cache thus stores the minimum costs found for any edge

on v,v0on the (0,0,0)cube K0(v,v0) = min(c(v,v0)).K0

is computed once for ab and abc motions and is speciﬁc

to robot root connector similarly to CM. The cache can

be applied to any edge on the lattice by expressing the

x,y,zcomponents of the vertex sockets relatively to the root

connector socket. However, unlike CM, computing the K0is

expensive. The ab cache involves 318 vertex combinations

and takes about 25h using a consumer grade laptop. The

abc cache involves 8268 vertex combinations and takes about

650h. To perform these computations we used the computing

cluster described in Section VI. The cache is asymptotically-

minimal because we are using the RRTStar planner, which

is asymptotically optimal. Cache optimality is subject to

number of trials and the RRTStar time out threshold (we

use 5 s). Fig. 6 shows how during both types of cache

computation the minimum costs over increasing number of

trials converges to the min over all trials.

2) Heuristics: In addition to edge costs, the cache allows

us to derive an informative Weighted Euclidean heuristic

hεto be used with A∗search. A heuristic h(v∈V)is an

estimation of the minimum cost of a feasible plan π, taking

(a) ab-cache (b) abc-cache

Fig. 6: % of edges whose mincosts over num trials are within an

error margin of 1% and 0.1% of min over all trials.

Algorithm 1: Iterative A*

Data: Π=hV,v0,Vg,Ki,RRTC

Result: π

1ValActions ←∅;c(A)←EstimatedCosts();Sol utionFound ← ⊥ ;

2while ¬SolutionFound do

3π,SolutionFound ←Astar(Π);

4if SolutionFound then

5validated =>;

6for a∈πdo

7if a∈ValActions then

8continue;

9f easible,cost ←RRTC onnect(hv,v0i ∈ a);

10 if f easible then

11 c(a)←cost;

12 ValActions ←a;

13 else

14 A←A\a;validated ← ⊥;

15 break;

16 if validated then

17 return π

18 return ∅// No Feasible Solution found

vto Vg. A heuristic is admissible if h(v)≤h∗(v)where h∗(v)

is the costs of a true optimal plan. A heuristic is consistent if

both h(v)≡0|∀v∈Vgand h(v)≤c(v,v0) +h(v0)|∀v∈V\Vg

where v0and vshare an edge. Consistency implies admissibil-

ity. Both properties are desirable as admissibility guarantees

optimality and consistency leads to fewer node expansions

and shorter search times. First we deﬁne a distance function

between two sockets s,s0. We then extend this deﬁnition to

deﬁne hεfor ab and abc graphs:

dist(s,s0) = ||(sx,sy,sz)−(s0

x,s0

y,s0

z)||2(5)

hε,ab(v∈Vab ,g) = ω·dist(vs,g)(6)

hε,abc(v∈Vabc ,g) = ω·min(dist(va,g),dist(vb,g)) (7)

The weight ωin Eqs. (6, 7) is determined using the cache K0.

For both graphs, it is the minimum (over all edges ([v,v0]∈

K0) ratio between edge costs K0(v,v0)and the maximum

distance between sockets involved with the edge. That is,

s,s0∈ {vs,v0

s}for ab cache and s,s0∈ {va,vb,v0

a,v0

b}for abc.

In other words, Eq 8 describes capturing the minimal costs

to travel the longest Euclidean distance.

ω=min

[v,v0]∈K0

K0(v,v0)

max

s,s0∈{v,v0}dist(s,s0)(8)

Due to the triangle inequality, an unweighted Euclidian

distance is a consistent heuristic if costs(v,v0)≥d(v,v0).

Therefore, assuming the costs c(v,v0) = K0(v,v0)are mini-

mal, we only need to ensure that |hε(v0)−hε(v)| ≤ c(v,v0)

to keep consistency. The trivial solution would be to make

ω≡0, but that would not be informative. Eq. (8) ensures that

(a) S6O5 lattice (b) S11O5 lattice

(c) S16O5 lattice (d) S21O5 lattice

Fig. 7: Cuboid lattice structure used for testing. For example, S11O5

corresponds to lattice of side length 11 and 5% of bounding 17 ×

17 ×17 cube volume being obstructed by obstacles. A video of

experiments is available here

there is no action for which the change in heuristic values

from vto v0is bigger than the cost of moving from vto v0.

The same logic applies to both graphs because we deﬁned the

distance function for abc vertices as the pair-wise minimum

distance between any sockets ((0,0,0,f),va,vb)∈ {v,v0}.

The admissibility and consistency of the heuristic using these

weights is only guaranteed up to the correctness of the

asymptotically-optimal cost estimates.

3) Iterative A∗:To reduce the number of calls to the joint

space planner, we perform iterative search and validation

using the following steps: 1) The graph is populated with

vertices and edges. Edge costs are estimated using K. 2) An

A∗search is performed on the graph. 3) We then validate

all edges in the path found using the RRTConnect planner.

During this step, the collision-free joint-trajectory Jis stored

with the edge and costs are updated with the duration of J.

This also includes removing edges, when joint state planner

ﬁnds no solution. 4) We repeat steps 2,3 using a persistent

graph data structure. That is, the edges are validated or

removed in every iteration. We are ﬁnished once we have

found a feasible path such that all edges in the path are

validated. Please see Algorithm 1 for details.

4) Optional Replanning Window: The ﬁrst time A∗search

is performed, the path is found using the estimated edge

costs, let the costs of this path be c(π∗). The algorithm 1

then goes on to validate the edges and call A∗again until the

whole path is feasible, resulting in a valid plan π. However, π

is not guaranteed to be the cheapest plan, because there might

be alternative cheaper paths using yet to be validated edges.

On the other hand, searching and validating edges takes time.

Hence, optimally, one could keep calling Iterative A∗for the

time duration c(π)−c(π∗). This replanning window allows

to only search for more optimal paths if a more optimal plan

is possible when taking into account the replanning costs. In

Section VI, we show that in the majority of experiments the

ﬁrst feasible plan returned by iterative A* is also the best

one once the replanning time effort is taken into account.

VI. EVALUATI ON

1) Experimental Setup: To evaluate our approach, we use

four lattice-structures as presented in Fig. 7. The cuboid

lattices range in size form 6 to 21 and are made up of

repeating sub-lattices. In order to obstruct inside and outside

the structure, we ﬁrst deﬁne a bounding cube. We do this

by adding 3 cube units along each direction, e.g. a bounding

cube for lattice of size 6 would be size 12. Then, for each

free cube-point we place an obstacle with a probability 0% to

50% in 5% increments, creating different obstacle densities.

Obstacle sizes are also chosen randomly using uniform

distribution between 0.035 to 0.28 m. Two random seeds

are used to generate worlds with >0% obstacle density. The

heuristic weights (eq. 8) were ωab =7.7 and ωabc =2.43

respectively.

All experiments have a randomized initial connector s0=

(sx,sy,sz,sf)and goal cube gwhich deﬁnes the goal con-

nectors Sg. We make sure s06∈ Sg. We run suites of 100

experiments per world size, obstacle density and obstacle

seed. This means data points with 0% obstacles have 100

trials, while others have 200. To run the experiments we

used a cluster with Intel Xeon E5-2640 processors running

at 2.60GHz. The memory limit by process was always set to

4 GBs and the time limit was 7,200 seconds.

2) Comparing ab and abc Graphs: Figure 8 shows that

the double socket abc hop model solves signiﬁcantly more

problems than the ab approach. This is due to two factors:

Firstly, the start and end joint-conﬁgurations of the robot

when using abc are more compact and are less likely to

be in collision with an obstacle hovering above the socket.

Secondly, not having to fully extend abc motions offer higher

maneuverability when avoiding obstacles, hence enabling

more feasible paths than ab which always needs to extend

fully between hops.

Fig. 8: Average Percentage of Solved, Unsolvable and Out of time

outcomes out of trials tested.

Despite abc-graph solving more problems, both methods

are worth considering as the double socket abc-graph relies

on a much more expensive cache computation. Also, the two

sockets abc-graphs are much bigger, see table I.

Lattice ab vertices ab edges abc vertices abc edges

S6O0 216 1056 503 3824

S6O25 82 170 211 940

S6O50 27 10 55 136

S11O0 918 4824 2219 17816

S11O25 386 892 963 4844

S11O50 99 40 222 434

S16O0 2400 13056 5879 48080

S16O25 1023 2532 2649 13948

S16O50 293 234 894 2730

S21O0 4950 27480 12203 100664

S21O25 2058 5096 5389 28312

S21O50 602 444 1570 4316

TABLE I: Vertex and Edge counts of graphs used in testing. Data is

for one random seed. The graph sizes shrink as obstacles obstruct

sockets or force collision with the robot.

3) Search Effort And Path Costs: Path execution costs

over a 100 trials are compared to the search effort spent

performing A∗and joint space validation in Fig. 9.

Fig. 9: Stacked average plan execution and search costs (split into

A∗and validation costs). The black line shows the average plan

costs of abc-graph only for problems solved by ab-graph as well.

As we add obstacles, and hence the graph size shrinks,

the robot must perform more complex motions, leading to

quickly increasing validation costs. A∗search time increases

for larger obstructed lattices as well. In general, to reduce

the computational effort, better validation strategies are re-

quired. The A∗effort could be reduced by using re-planning

algorithms like LPA∗, but that would not affect validation.

The double socket abc-graph not only solves all problems

that are solved via the single socket ab graph, but also ﬁnds

shorter paths. This is shown via the superimposed black lines

denoting abc performance on problems solved by ab. This

is expected as the ab motion involves the robot returning

to an all-zero joint state. Thus moving longer distances in

joint-space and encountering more obstacles. Furthermore,

for both graphs, the plan costs increase with obstacle density,

but start decreasing as it approaches 50%. This is consistent

with the graph size results in Table I as only problems with

start and end sockets that are close together are solvable.

4) Time Saved By Replaning: In Fig. 10 we show how

re-planning performs, see Sec. V-B.4. In the ﬁgure, savings

mean the difference between the ﬁrst feasible plan and the

best ever found feasible plan. All results are normalized

against the ﬁrst feasible plan cost. For most problems the

ﬁrst found feasible solution is also the best one when taking

into account the re-planning search effort. However, actual

results are speciﬁc to the robot capabilites.

Fig. 10: Average time savings and re-planning effort, in terms of

the ratio against the ﬁrst feasible plan, over all trials.

VII. CONCLUSIONS AND FUTURE WORK

In this paper, we have presented two ways to formulate

a graph to perform hybrid discrete-continuous path planning

for an inchworm robot on obstructed lattice structures. We

showed that for both graphs combining iterative A* search

and a pre-computed cache structure scales signiﬁcantly better

than purely online validation strategies.

The double socket abc graph formulation clearly outper-

forms the single socket ab, both in terms of problems solved

and path costs. Although, the abc-graph requires a more

expensive ofﬂine cache computation, this is reasonable con-

sidering target applications are heavy duty tasks like nuclear

decommissioning. Furthermore, the ofﬂine cache is subject

to exploitation of structure-speciﬁc symmetries and can be

further optimized if limited inverse kinematics solutions are

considered combinatorially instead of stochastically.

For both graphs, the search effort is mostly dominated by

continuous motion planning costs. Future work could explore

optimization-based planners like CHOMP [4] to address this.

Additionally, algorithms like LPA∗should be used when the

search effort is signiﬁcant compared to the validation effort.

However, the most interesting research direction is to com-

bine robot path planning with structure assembly planning.

The ability to accurately estimate traverse costs midway

through assembly could prove invaluable for solving the

assembly sequencing problem.

VIII. ACKN OWLEDGME NT S

This work is partially supported by InnovateUK Connect-

R (TS/S017623/1).

REFERENCES

[1] C. Pan, Y. Han, and J. Lu, “Design and optimization of lattice

structures: A review,” Applied Sciences, vol. 10, no. 18, 2020.

[2] K. H. Petersen, N. Napp, R. Stuart-Smith, D. Rus, and M. Kovac, “A

review of collective robotic construction,” Science Robotics, vol. 4,

no. 28, 2019.

[3] N. Gershenfeld, M. Carney, B. Jenett, S. Calisch, and S. Wilson,

“Macrofabrication with digital materials: Robotic assembly,” Archi-

tectural Design, vol. 85, no. 5, pp. 122–127, 2015.

[4] S. Leder, R. Weber, D. Wood, O. Bucklin, and A. Menges, “Distributed

Robotic Timber Construction,” in Acadia, 2019, pp. 510–519.

[5] K. Petersen, R. Nagpal, and J. Werfel, “TERMES: An Autonomous

Robotic System for Three-Dimensional Collective Construction,” in

Robotics: Science and Systems VII, vol. 7, 2011, pp. 257–264.

[6] B. Jenett, A. Abdel-Rahman, K. Cheung, and N. Gershenfeld, “Ma-

terial–Robot System for Assembly of Discrete Cellular Structures,”

IEEE Robotics and Automation Letters, vol. 4, no. 4, jul 2019.

[7] B. Jenett and K. Cheung, “BILL-E: Robotic platform for locomotion

and manipulation of lightweight space structures,” 25th AIAA/AHS

Adaptive Structures Conference, 2017, pp. 1–12, 2017.

[8] J. Roberts, S. Franco, A. Stokes, and S. Bernardini, “Autonomous

Building of Structures in Unstructured Environments via AI Planning,”

Proceedings of the International Conference on Automated Planning

and Scheduling, vol. 31, pp. 491–499, 2021.

[9] H. L. Rodriguez, B. Bridge, and T. P. Sattar, “Climbing ring robot for

inspection of offshore wind turbines,” in Advances in Mobile Robotics.

WORLD SCIENTIFIC, aug 2008, pp. 555–562.

[10] W. Jeon, I. Kim, J. Park, and H. Yang, “Design and control method

for a high-mobility in-pipe robot with ﬂexible links,” Industrial Robot,

vol. 40, no. 3, pp. 261–274, 2013.

[11] G. Paul, P. Quin, A. W. K. To, and D. Liu, “A sliding window

approach to exploration for 3D map building using a biologically

inspired bridge inspection robot,” 2015 IEEE International Conference

on Cyber Technology in Automation, Control and Intelligent Systems,

IEEE-CYBER 2015, no. June 2015, pp. 1097–1102, 2015.

[12] H. Zhang, J. Zhang, G. Zong, W. Wang, and R. Liu, “Sky cleaner 3,”

IEEE Robotics and Automation Magazine, vol. 13, no. 1, 2006.

[13] K. Kotay and D. Rus, “The Inchworm Robot: A Multi-Functional

System,” Autonomous Robots, vol. 97, no. 1-4, pp. 131–141, 2000.

[14] B. Jenett, C. Cameron, F. Tourlomousis, A. P. Rubio, M. Ochalek, and

N. Gershenfeld, “Discretely assembled mechanical metamaterials,”

Science Advances, vol. 6, no. 47, pp. 1–12, 2020.

[15] C. H. J. Yang, G. Paul, P. Ward, and D. Liu, “A path planning approach

via task-objective pose selection with application to an inchworm-

inspired climbing robot,” in IEEE/ASME International Conference on

Advanced Intelligent Mechatronics, AIM, vol. 2016-Sept, pp. 401–406.

[16] H. Zhu, S. Gu, L. He, Y. Guan, and H. Zhang, “Transition analysis

and its application to global path determination for a biped climbing

robot,” Applied Sciences (Switzerland), vol. 8, no. 1, jan 2018.

[17] T. L. Lam and Y. Xu, “Climbing strategy for a ﬂexible tree climbing

robot - treebot,” IEEE Transactions on Robotics, vol. 27, no. 6, pp.

1107–1117, dec 2011.

[18] T. Lam and Y. Xu, “Motion planning for tree climbing with inchworm-

like robots,” Journal of Field Robotics, vol. 30, no. 1, pp. 87–101,

2013.

[19] D. Pagano and D. Liu, “An approach for real-time motion planning of

an inchworm robot in complex steel bridge environments,” Robotica,

vol. 35, no. 6, pp. 1280–1309, 2017.

[20] D. L. Maria Fox, “The automatic inference of state invariants in tim,”

Journal of Artiﬁcial Intelligence Research, vol. 9, no. 1, pp. 367–421,

1998.

[21] C. Dornhege, P. Eyerich, and T. Keller, “Semantic Attachments for

Domain-Independent Planning Systems,” in Proceedings of the 19th

International Conference on Automated Planning and Scheduling

(ICAPS-09), 2009, pp. 114–121.

[22] S. Bernardini, M. Fox, D. Long, and C. Piacentini, “Boosting search

guidance in problems with semantic attachments,” Proceedings of

the International Conference on Automated Planning and Scheduling,

vol. 27, no. 1, pp. 29–37, Jun. 2017.

[23] S. Koenig, M. Likhachev, and D. Furcy, “Lifelong Planning A*,”

Artiﬁcial Intelligence, vol. 155, no. 1-2, pp. 93–146, may 2004.

[24] A. Stentz, “The Focussed D* Algorithm for Real-Time Replanning,”

in International Joint Conference on Artiﬁcial Intelligence (IJCAI),

1995, pp. 1652 – 1659.

[25] “Connect-r,” https://www.ross-robotics.co.uk/news/bringing-structure-

to-unstructured- environments, accessed: 2022-06-06.

[26] I. A. Sucan and S. Chitta, “Moveit motion planning framework,” 2018.

[27] P. Beeson and B. Ames, “TRAC-IK: An open-source library for

improved solving of generic inverse kinematics,” in IEEE-RAS In-

ternational Conference on Humanoid Robots, vol. 2015-Dec. IEEE

Computer Society, pp. 928–935.

[28] J. J. Kuffner and S. M. LaValle, “RRT-connect: An efﬁcient approach

to single-query path planning,” in IEEE international conference on

robotics and automation (ICRA), vol. 2. IEEE, 2000.

[29] S. Karaman, M. R. Walter, A. Perez, E. Frazzoli, and S. J. Teller, “Any-

time motion planning using the RRT,” in International Conference on

Robotics and Automation, ICRA. IEEE, 2011, pp. 1478–1483.

[30] I. A. Sucan, M. Moll, and L. E. Kavraki, “The Open Motion Planning

Library,” IEEE Robotics & Automation Magazine, vol. 19, no. 4, pp.

72–82, dec 2012.