Journal Title XX(X):1–22 Motion Planning for Variable Topology ©The Author(s) 2021 Trusses: Reconfiguration and Locomotion

Chao Liu1, Sencheng Yu2, and Mark Yim1

Abstract Truss robots are highly redundant parallel robotic systems and can be applied in a variety of scenarios. The variable topology truss (VTT) is a class of modular truss robot. As self-reconfigurable modular robots, variable topology trusses are composed of many edge modules that can be rearranged into various structures with respect to different activities and tasks. These robots are able to change their shapes by not only controlling joint positions which is similar to robots with fixed morphologies, but also reconfiguring the connections among modules in order to change their morphologies. Motion planning is the fundamental to apply a VTT robot, including reconfiguration to alter its shape, and non-impact locomotion on the ground. This problem for VTT robots is difficult due to their non-fixed morphologies, high dimensionality, the potential for self-collision, and complex motion constraints. In this paper, a new motion planning framework to dramatically alter the structure of a VTT is presented. It can also be used to solve locomotion tasks much more efficient compared with previous work. Several test scenarios are used to show its effectiveness.

Keywords Modular Robots, Parallel Robots, Motion Planning, Reconfiguration Planning, Locomotion

1 Introduction Self-reconfigurable modular robots consist of repeated building blocks (modules) from a small set of types with uniform docking interfaces that allow the transfer of mechanical forces and moments, electrical power, and communication throughout all modules (Yim et al. 2007). These systems are capable of reconfiguring themselves in order to handle failures and adapt to different tasks. Many self-reconfigurable modular robots have been developed, with the majority being lattice or chain type Figure 1. The hardware prototype of a VTT in octahedron systems. In lattice modular robots, such as Telecubes (Suh configuration is composed of 12 members. Note that at least 18 et al. 2002) and Miche (Gilpin et al. 2008), modules are members are required for topology reconfiguration Spinos et al. regularly positioned on a three-dimensional grid. Chain (2017). modular robots, such as PolyBot (Yim et al. 2000), M- Tran (Murata et al. 2002), and CKBot (Yim et al. 2009), geometry truss robots with additional capability to self- consist of modules that form tree-like structures with reconfigure the connection between members to alter the kinematic chains that can act like articulated robot arms. truss topology (Spinos et al. 2017; Jeong et al. 2018). One Some systems, such as SUPERBOT (Salemi et al. 2006) arXiv:2108.00309v1 [cs.RO] 31 Jul 2021 of the current hardware prototypes is shown in Figure1. and SMORES-EP (Liu et al. 2019a) are hybrid and move A significant advantage for self-reconfigurable modular like chain systems for articulated tasks but reconfigure using robots over other robots with fixed morphologies is their lattice-like actions. versatility, namely they are able to adapt themselves into Modular truss robots are different from lattice and chain different morphologies with regard to different requirements. type systems in that the systems are made up of beams For example, a VTT in which the members form a broad that typically form parallel structures. The variable geometry supported structure is well-suited for shoring damaged truss (VGT) (Miura 1984) is a modular robotic truss system composed of prismatic joints as truss members (modules), and examples include TETROBOT by Hamlin 1University of Pennsylvania, USA and Sanderson(1997), Odin by Lyder et al.(2008), and 2Texas A&M University, USA Linear Actuator Robots by Usevitch et al.(2017). These truss members alter their length to perform locomotion or shape- Corresponding author: Chao Liu, GRASP Laboratory and The Department of Mechanical morphing tasks. Another similar hardware is Intelligent Engineering and Applied Mechanics, University of Pennsylvania, 220 Precision Jigging Robots by Komendera and Correll(2015). South 33rd St., Philadelphia, PA, USA. The variable topology truss (VTT) is similar to variable Email: [email protected] 2 Journal Title XX(X) buildings to enable rescue operations. Another truss with Self-reconfiguration enables a VTT to be better suited some members protruding to form an arm may have a large for a variety of tasks by dramatically changing its shape reachable workspace that is good at manipulation tasks. and topology. Locomotion is one important task in many However, a fundamental complication with VTT systems scenarios. For example, in a search and rescue mission, a comes from the motion of the complex parallel structures that VTT may need to flexibly locomote over some terrain to may result in self-collision. Developing self-collision-free reach a destination and then reconfigure into a tower for motion plans is difficult due to the large number of degrees shoring. The locomotion process can be achieved by moving of freedom leading to large search space. nodes sequentially that can interact with the environment. A truss is composed of truss members (beams) and One way to move would be quasi-statically to reduce nodes (the connection points of multiple beams). A VTT potentially damaging impacts with the ground (Park et al. is composed of edge modules each of which is one active 2019). prismatic joint member and one passive joint end that This paper builds on a sampling-based motion planning can actively attach or detach from other edge module framework presented in our previous conference paper (Liu ends (Spinos et al. 2017). The configuration can be fully et al. 2020). In this conference paper, we showed that the defined by the set of member lengths and their node geometry reconfiguration planning can be solved efficiently assignments at which point the edge modules are joined. A by explicitly computing the configuration space for a node is constructed by multiple edge module ends using a group of nodes resulting in smaller sampling space and a linkage system with a passive rotational degree of freedom. simpler collision model. We also extended the approach by The node assignments define the topology or how truss edge Liu et al.(2020) to compute all separated enclosed free modules are connected, and the length of every member spaces for a single node so that a sequence of topology defines the shape of the resulting system (Liu and Yim reconfiguration actions can be computed by exploring these 2019). Thus, there are two types of reconfiguration motions: spaces using a simple rule. This paper adds significantly geometry reconfiguration and topology reconfiguration. beyond the conference version in the following. We first Geometry reconfiguration involves moving positions of extend our configuration space algorithm to take the nodes by changing length of corresponding members and geometry size of the robot into account. Then more physical topology reconfiguration involves changing the connectivity hardware constraints are incorporated into the framework. A among members. sampling method is presented to generate enough samples There are some physical constraints for a VTT to execute in every enclosed free space that can provide topology geometry reconfiguration and topology reconfiguration. A reconfiguration actions as many as possible and also cover VTT has to be a rigid structure in order to maintain its the space as much as possible. Then these samples can shape and be statically determinant. A node in a VTT must be used to search a sequence of actions for a given be of degree three, so has to be attached by at least three reconfiguration task. Finally, we present a novel locomotion members to ensure its controllability. In addition, A VTT framework based on this motion planning approach which requires at least 18 members before topology reconfiguration provides more robust and efficient performance compared is possible (Spinos et al. 2017). Thus, motion planning with existing locomotion planning algorithms for truss for VTT systems has to deal with at least 18 dimensions robots. and typically more than 21 dimensions. These constraints The rest of the paper is organized as follows. Section2 complicate the motion planning problem. reviews relevant and previous works and some necessary As VTTs are inherently parallel robots, it is much easier to concepts are introduced in Section3 as well as the problem solve the inverse kinematics than the forward kinematics. So, statement, including reconfiguration and locomotion. Vari- for the geometry reconfiguration, rather than doing motion able topology truss kinematics is introduced in Section4. planning for the active degrees of freedom — the member Section5 presents the geometry reconfiguration algorithm lengths — we plan the motion of the nodes and then do with motion of multiple nodes involved considering physical the inverse kinematics to easily determine member lengths. hardware geometry and constraints. Section6 introduces the However, the arrangement of edge modules in a VTT can approach to verify whether a topology reconfiguration action result in a complicated configuration space even for a single is needed and the planning algorithm to do topology recon- node while keeping the others rigid, and motions of multiple figuration. The locomotion planning approach is discussed nodes are strongly coupled, namely moving one node can in Section7. Our framework is demonstrated in several test significantly affect the configuration space of other nodes. scenarios in Section8. Finally, Section9 talks about the Some desired node motions are impossible to do without conclusion and some future work. self-collision. These can become possible with topology reconfiguration. For example, a single node controlled by 2 Related Work enough edge modules can be split into a pair of nodes in order to go around some internal blocking members and In order to enable modular robotic systems to adapt them- then merge back to an individual node. This process requires selves to different activities and tasks, many reconfigu- motion planning for two nodes at the same time. In addition ration motion planning algorithms have been developed to shape morphing tasks, this motion planning problem is over several decades for a variety of modular robotic sys- also the fundamental to solve locomotion tasks for a VTT. tems (Casal and Yim 1999; Butler et al. 2004; Hou and Multiple nodes have to move in cooperation in order to Shen 2014; Liu et al. 2019a). The planning frameworks move the robot on some terrain while maintaining physical are for topology reconfiguration where undocking (discon- constraints. necting two attached modules) and docking (connecting Liu et al. 3 two modules) actions are involved. There are also some reconfiguration actions can then be generated which can approaches for shape morphing and manipulation tasks, achieve behaviors that are similar to the DNA replication including inverse kinematics for highly redundant chain process. using PolyBot (Agrawal et al. 2001), constrained optimiza- The VTT locomotion process is similar to a locomotion tion techniques with nonlinear constraints (Fromherz et al. mode of some VGTs which is accomplished by tipping 2001), and a quadratic programming approach with linear and contacting the ground. TETROBOT systems and others constraints to solve control and motion planning simultane- have been shown with this mode in simulation with ously in real time (Liu and Yim 2021). In these works, there generated paths for moving nodes (Woo Ho Lee and are no topology reconfiguration actions involved, but compli- Sanderson 2002; Abrahantes et al. 2010). These works divide cated kinematic structures and planning in high dimensional the locomotion gait into several steps, but they have to spaces need to be considered. However, these methods are compute the motion of nodes beforehand which cannot be not applicable to variable topology truss systems which have applied to arbitrary configurations. Optimization approaches a very different morphology and connection architecture. have been used for locomotion planning. Usevitch et al. Indeed the physical constraints and collision models are (2017) formulates the locomotion process as a quadratic significantly different from all of the previous lattice and program by constraining the motion of the center of mass. chain type systems. The objective function is related to the velocity of each Some approaches have been developed for VGT systems node. A more complete quadratic programming approach that are similar to VTT systems but without topology to locomotion is presented by Usevitch et al.(2020) to reconfiguration capability. Hamlin and Sanderson(1997) generate discrete motions of nodes in order to follow a presented a kinematic control but is limited to tetrahedrons or given trajectory or compute a complete gait cycle. More octahedrons. Usevitch et al.(2017) introduced linear actuator hardware constraints were considered, including length and robots (LARs) as well as a shape morphing algorithm. collision avoidance. However, the approach has to solve These systems are in mesh graph topology constructed by an optimization problem in high dimensional space, and it multiple convex hulls, and therefore self-collision can be also has to deal with non-convex and nonlinear constraints avoided easily. However, this does not apply to VTT systems which may cause numerical issues and is limited to a fully because edge modules span the space in a very non-uniform connected five-node graph in order to avoid incorporating the manner. There has been some work on VTT motion planning. manipulability constraints into the quadratic program. Park Retraction-based RRT algorithm was developed by Jeong et al.(2019) extends this optimization-based approach by et al.(2018) in order to handle this high dimensional problem preventing a robot from receiving impacts from the ground. with narrow passage difficulty which is a well-known issue Incorporated with a polygon-based random tree search in sampling-based planning approaches; nevertheless, this algorithm to output a sequence of supporting polygons approach is not efficient because it samples the whole by Park et al.(2020), a VTT can execute a locomotion task workspace for every node and collision checking needs to be in an environment. However, in these quadratic programs, done for every pair of members. Also, sometimes waypoints numerical differentiation is required to relate some physical have to be assigned manually. Liu and Yim(2019) presented constraints with these optimized parameters whenever a reconfiguration motion planning framework inspired by solving the problem. A locomotion step has to be divided the DNA replication process — the topology of DNA can into multiple phases leading to more constraints. Also, these be changed by cutting and resealing strands as tanglements approaches are not guaranteed to provide feasible solutions form. This work is based on a new method to discretize the and they are also time-consuming to solve. In this paper, we workspace depending on the space density and an efficient present a new locomotion planning solution based on our way to check self-collision. Both topology reconfiguration efficient geometry reconfiguration planning algorithm. Our actions and geometry reconfiguration actions are involved if solution can solve the problem much faster and more reliably needed. However, only a single node is involved in each step under several hardware constraints compared with previous and the transition model is more complicated, which makes works. This approach can be applied to any arbitrary truss the algorithm limited in efficiency. robot. Liu et al.(2020) presented a fast algorithm to compute the reachable configuration space of a given node in a VTT 3 Preliminaries and Problem Statement which is usually a non-convex space and this space can be then decomposed into multiple convex polyhedrons so that a A VTT can be represented as an undirected graph G = simple graph search algorithm can be applied to plan a path (V,E) where V is the set of vertices of G and E is the set of for this node efficiently. However, multiple nodes are usually edges of G: each member can be regarded as an undirected involved in shape morphing. In this paper, we first extend this labeled edge e ∈ E of the graph and every intersection approach to compute the obstacles for multiple nodes so that among members can be treated as a vertex v ∈ V of the the search space can be decreased significantly. In addition, graph denoting a node. The Cartesian coordinates of a node only the collision among a small number of edge modules v ∈ V is encoded as its property Pos such that v [Pos] = 3 v needs to be considered when moving multiple nodes at the [vx, vy, vz]| ∈ R . Let q = v[Pos] and the configuration same time. Hence sampling-based planners can be applied space of node v denoted as Cv is simply R3. In this way, efficiently. The idea has been discussed briefly by Liu et al. the state of a member e = (v1, v2) ∈ E where v1 and v2 are (2019b). For some motion tasks, topology reconfiguration is two vertices of edge e can be fully defined by qv1 and qv2 . required. An updated algorithm is developed to compute the The position of a given node v ∈ V is controlled by changing whole not fully connected free space and required topology the lengths of all attached members denoted as Ev ⊆ E. 4 Journal Title XX(X)

(a) (b)

Figure 2. (a) Given node v0, one of its neighbors v1 and a v member (v6, v8) can define the blue polygon which is part of 0 v0 Figure 3. Cfree(q ) is bounded by polygons and workspace v0 v0 Cobs. (b) The obstacle region Cobs is composed of polygons and boundaries. 3 v the leftover space of R is Cfree.

The geometry reconfiguration motion planning of a VTT is achieved by planning the motion of the involved nodes then determining the required member length trajectories since it is easier to solve the inverse kinematics problem. Given a node v ∈ V in G = (V,E), the state of every Figure 4. A single node with six members can be split into a v v v member e ∈ E , denoted as A (q ), can be altered by pair of nodes and two separate nodes can also merge into an changing qv. The obstacle region of this node which includes individual node. v v self-collision with other members as obstacles Cobs ⊆ C = R3 is defined as • Geometry Reconfiguration. For a set of n nodes v v 3 v v v Cobs = {q ∈ R |A (q ) ∩ O 6= ∅} (1) {vt ∈ V |t = 1, 2, ··· , n}, compute paths τt : [0, 1] → vt vt vt C such that τt(0) = q and τt(1) = q in which v v free i g in which O is the obstacle for E . Liu et al.(2020) proved vt t = 1, 2 ··· , n, qi is the initial position of vt and that this obstacle region is fully defined by the states of ∀e ∈ vt q is the goal position of vt, while satisfying all v g E \ E and composed of multiple polygons. For a simple constraints. v0 VTT shown in Figure 2a, the obstacle region Cobs is shown • Topology Reconfiguration. Compute the topology in Figure 2b. The free space of node v is just the leftover reconfiguration actions, Merge and Split, and find configurations denoted as collision-free path(s) to move a node v from its initial v v v 3 v position qi to its goal position qg , while satisfying all Cfree = R \Cobs (2) constraints. However, Cv may not be fully connected and may be free The shape of a VTT G = (V,E) can be regarded as a partitioned by Cv . Only the enclosed subspace containing obs polyhedron with flat polygonal facets formed by members. qv which is denoted as Cv (qv) is free for node v to move. free This polyhedron consists of vertices (a subset of V ), edges, A fast algorithm to compute the boundary of this subspace is facets F , and an incidence relation on them, e.g. every edge presented by Liu et al.(2020). For example, given the VTT is incident to two vertices and every edge is incident to in Figure 2a, Cv0 — the free space of v — is partitioned free 0 two facets (Kettner 1999). One of the facets f ∈ F is the by Cv0 in Figure 2b, and the subspace Cv0 (qv0 ) is shown in obs free current support polygon. In a rolling locomotion step, the Figure3. support polygon is changed from f to an adjacent facet f 0. Cv is usually partitioned by Cv into multiple enclosed free obs As mentioned, impact from the ground can damage the robot, subspaces, and it is impossible to move v from one enclosed so the stability criterion has to be maintained during the subspace to another one without topology reconfiguration. whole process. The locomotion problem for a given VTT The physical system constraints (Spinos et al. 2017) G = (V,E) can be stated in the following: allow two atomic actions on nodes that enable topology reconfiguration: Split and Merge. Since the physical • Non-impact Rolling Locomotion. Compute the system must be statically determinate with all nodes of motion of a set of nodes in V to change the support degree three, a node v must be composed of six or more polygon from f ∈ F to a desired adjacent facet f 0 ∈ edge modules to undock and split into two new nodes v0 and F without receiving impacts from the ground while v00, and both nodes should still have three or more members. satisfying all constraints. This process is called Split. Two separate nodes are able to merge into an individual one in a Merge action. The 4 Robot Kinematics and Hardware simulation of these two actions is shown in Figure4. Hence, in a topology reconfiguration process, the number of nodes Constraints can change, but the number of members that are physical Given a VTT G = (V,E), motion of nodes are controlled elements remains constant. by all attached actuated members and the system is usually In this work, given a VTT G = (V,E), the reconfiguration an overconstrained parallel robot. The set of all nodes V is planning problem can be stated as the following: separated into two groups: VF and VC where VF contains Liu et al. 5

in which h i| ˙ α ˙ α ˙ α ˙ Lα = ˆl| ˆl| ··· ˆl| 1 2 N 3N×1  αˆ|  l1 0 ··· 0  0 αˆl| ··· 0   2  Bα =  . . . .   . . .. .  0 0 ··· αˆl| N N×3N  v¯ vˆα v¯ vˆα v¯ vˆα | A = q α − q 1 q α − q 2 ··· q α − q N α N×3

If another controlled node v¯β ∈ NG(¯vα), namely v¯β is adjacent to v¯α, then we have

¯˙ v¯β v¯α lαβ =q ˙ − q˙ (6) Figure 5. A VTT is composed of twelve members. Currently, the motion of node v1 and node v4 are under control by seven Combining Eq. (5) and Eq. (6), the following equation of blue members. motion can be derived BL˙ = Ap˙ (7) all the fixed or stationary nodes and VC contains all the in which controlled nodes. For example, given the VTT shown in p =  (qv¯1 )| (qv¯2 )| ··· (qv¯Λ )| | Figure5, when controlling the motion of node v1 and node h i| ˙ ˙ | ˙ | ˙ | ¯˙ v4, VC = {v1, v4} and VF = {v0, v2, v3, v5}, and this is L = L1 L2 ··· LΛ ··· lαβ ··· a 6-DOF system since there are two controlled nodes. In B = diag(B ,B , ··· ,B , ··· ,I, ··· ) A = [A , A ]| addition, this system is overconstrained and the motion of 1 2 Λ 1 2 A1 = diag(A1,A2, ··· ,Aα, ··· ,Aβ, ··· ,AΛ) two nodes are controlled by seven members. Note that VF and V are not constant and they can be changed during the  . . . . .  C . . . . . motion of a VTT (reconfiguration and locomotion).   A2 =  03×3α I3×3 03×(β−α−1) −I3×3 03×3(Λ−β)   . . . . .  . . . . .

4.1 Robot Kinematics The size of A2 is determined by the number of controlled vertices and connection link vectors (if there are Given V with Λ nodes under control, the position vector of C ω connection link vectors, A is a 3ω × 3Λ matrix). For the this system is simply the stack of qv where v ∈ V , and this 2 C system shown in Figure5, A = [I, −I] since there are system is controlled by all members that are attached with 2 3×6 S v only two controlled nodes and one connection link vector. It these nodes, namely E . Let lij be the link vector v∈VC can be shown that B has full rank as long as there are no zero- from a controlled node v pointing to any node v . There i j length members whereas A may not have full rank. This does are two types of link vectors: 1. if v ∈ V , then l is an j F ij make sense because there must exist a set of link velocities attachment link vector; 2. if v ∈ V , then l is a connection j C ij given the velocities of all controlled nodes, but some link link vector. The link vector satisfies the following equation: velocities may result in invalid motions of nodes since the system can be overconstraint. vj vi lij = q − q (3) We can rearrange Eq. (7) in two ways: + L˙ = B Ap˙ = JBAp˙ (8a) in which v ∈ V . Taking time derivative of Eq. (3) for an i C p˙ = A+BL˙ = J L˙ (8b) attachment link vector and a connection link vector yields AB where B+ and A+ are the pseudo-inverse of B and A respectively, and both JBA and JAB matrices are the l| l˙ = (qvi − qvj )|q˙vi ∀v ∈ V ij ij j F (4a) Jacobian. We use these two equations to describe the relationship between the link velocities and the controlled node velocities. JBA is always defined (as long as there is no ˙ vj vi lij =q ˙ − q˙ ∀vj ∈ VC (4b) zero-length member), but JAB may not be defined. Given p˙ is known, Eq. (8a) gives the minimum norm solution to Eq. (7), namely minimizing kLk˙ . On the other hand, Eq. (8b) results Assume VC = {v¯α|α = 1, 2, ··· , Λ}, and for a controlled in the unique least square solution to Eq. (7) if L˙ is known, node v¯α, all the fixed nodes in its neighborhood NG(¯vα) namely minimizing kBL˙ − Ap˙k. α α α are denoted as vˆ1 , vˆ2 , ··· , vˆN , and the corresponding αˆ αˆ αˆ attachment link vectors are denoted as l1, l2, ··· , lN . 4.2 Hardware and Environmental Constraints Eq. (4a) is true for any αˆl where t ∈ [1,N], so we can t A VTT has to maintain some hardware constraints during its rewrite it into the following form for a controlled vertex v i motion, and this includes the limitations from the hardware, the stability and controllability of the system, and impact v¯α BαL˙ α = Aαq˙ (5) avoidance in reconfiguration and locomotion activities. 6 Journal Title XX(X)

4.2.1 Length Constraints In a VTT, each edge module configuration space is R3. Apparently, the configuration has an active prismatic joint called Spiral Zipper (Collins space for n nodes is R3n. When multiple nodes are involved, and Yim 2016) and this joint is able to achieve a high the motion planning problem will be in high-dimensional extension ratio as well as form a high strength to weight space. Our strategy to avoid this high dimensionality is to ratio column. The mechanical components determine the divide the moving nodes into multiple groups and each group minimum member length and the total material determines contains one or a pair of nodes. The motion planning space the maximum member length. Hence, in a VTT G = (V,E), for each group is either in R3 or R6. Even with lower- ∀e = (vi, vj) ∈ E, we have the following constraint dimensional space, it is still a challenge to search for a valid solution. First, it is not efficient to search the whole 2 2 vi vj 3 6 Lmin ≤ kq − q k ≤ Lmax (9) R or R and narrow passage is a significant problem when applying rapidly-exploring random tree (RRT) algorithm. In 4.2.2 Collision Avoidance During the motion of a VTT, addition, it is difficult to guarantee the collision avoidance we have to avoid the collision between members. The for a given motion by simply discretizing the motion to some distance between every two members have to be greater than resolution and checking states at that resolution because the ¯ dmin, the diameter of an edge module which is a cylinder (or case that a member goes across another member may not the sum of the radius of a node and the radius of an edge be detected, or very fine resolution has to be used which module). The minimum distance between member (vi, vj) will slow down the planner significantly. We overcome these and (vm, vn) can be expressed as issues by computing the free space of the group in advance so that the sampling space is decreased significantly. min k(qvi + α(qvj − qvi )) − (qvm + γ(qvn − qvm ))k (10) 5.1 Obstacle Region and Free Space in which α, γ ∈ (0, 1). This is not easy to compute and can be more complicated when both members are actuating. In A group can contain either one node or a pair of nodes. Section5, we will show that our approach doesn’t need to Given a VTT G = (V,E), for an individual node v ∈ V , Cv Cv (qv) solve this problem. We also need to ensure the angle between an efficient algorithm to compute obs and free with connected members remains above a certain value due to the its boundary is introduced by Liu et al.(2020). If there are mechanical design of the node. The angle constraint between two nodes vi ∈ V and vj ∈ V in a group, then any collision among members in Evi and Evj is treated as self-collision member (vi, vj) and (vi, vk) can be expressed as inside the group, and all the members in E \ (Evi ∪ Evj ) v  vj vi vk vi  i (q − q ) • (q − q ) define the obstacle region of this group denoted as Cbobs (the ¯ v arccos v v v v ≥ θmin (11) j kq j − q i kkq k − q i k obstacle region of vi in the group) and Cbobs (the obstacle region of vj in the group) respectively, namely 4.2.3 Stability The truss structure of a VTT is meant to vi  vi 3 vi vi vi,vj be statically stable under gravity when interacting with the Cbobs = q ∈ R |A (q ) ∩ O 6= ∅ environment and sufficient constraints between the robot and v C j = qvj ∈ 3|Avj (qvj ) ∩ Ovi,vj 6= ∅ the environment must be satisfied to ensure the location of bobs R the structure is fully defined. At least three still nodes should in which Ovi,vj is formed by every e ∈ E \ (Evi ∪ Evj ). be on the ground in order to form a valid support polygon. Then the free space of vi and vj in the group can be derived For the VTT shown in Figure5, v0, v2, v3, and v5 are as stationary on the ground to form the current support polygon. In addition, the center of mass of a VTT as represented on the vi 3 vi Cbfree = R \ Cbobs (13) ground has to be inside this support polygon. Furthermore, Cvj = 3 \ Cvj (14) no collision is allowed between a VTT and the environment, bfree R bobs and the simplest condition is that all nodes have to be above Using the same boundary search approach in Liu et al. the ground. vi vi (2020), the boundary of Cbfree(q ) — the enclosed subspace 4.2.4 Manipulability In order to control the motion of containing the current position of node vi — can be obtained vj vj nodes, a VTT has to maintain an amount of manipulability efficiently. Similarly, the boundary of Cbfree(q ) can be for these moving nodes. Given a VTT G = (V,E) and obtained. For example, given the VTT shown in Figure6, if v0 v0 v1 v1 the current controlled node set VC , the Jacobian JAB can node v0 and v1 form a group, then Cbfree(q ) and Cbfree(q ) be derived as shown in Eq. (8b). By applying singular value decomposition on JAB, its maximum singular value σmax(JAB) and minimum singular value σmin(JAB) can be derived, and the manipulability of current moving nodes can be constrained as

σmin(JAB) µ = ≥ µ¯min (12) σmax(JAB)

5 Geometry Reconfiguration The overall shape of a VTT is altered by moving nodes Figure 6. A VTT is composed of 21 edge modules with 10 around in the workspace. For an individual node, its nodes among which v0 and v1 form a group. Liu et al. 7

(a) (b)

v0 v0 Figure 7. (a) Cbfree(q ) is computed with all members v1 v1 controlling v1 ignored. (b) Cbfree(q ) is computed with all members controlling v0 ignored.

v0 Figure 8. One obstacle polygon of Cobs shown in Figure 2a becomes a polyhedron bounded by five polygons if the size of

v0 v0 VTT components is considered. can be computed and shown in Figure7. Cfree(q ) contains v1 some space on the right side of v1 because E is ignored. This space informs that it is possible to move v0 to some locations which are currently blocked by v1 since v1 can be moved away. It is guaranteed that as long as v0 is moving v0 v0 inside Cbfree(q ) (the space shown in Figure 7a), there must be no collision between any member in Ev0 and any member in E \ (Ev0 ∪ Ev1 ). Similarly, no collision between any member in Ev1 and any member in E \ (Ev0 ∪ Ev1 ) can v1 v1 happen if v1 is moving inside Cbfree(q ) (the space shown in Figure 7b). In this way, when planning the motion of node v0 (a) (b) and v1 using RRT, the sample will only be generated inside v v Cb 0 (qv0 ) and Cb 1 (qv1 ), and we only need to consider self- free free Figure 9. (a) Detailed illustration of the formation of the collision in the group, namely the collision among members obstacle polyhedron. (b) Close view of the obstacle polyhedron. in Ev0 ∪ Ev1 . There is a special case when these two nodes in the group are connected by a member. Both ends of the member are moving which is not considered by our obstacle respectively model. This is an extra case when doing collision check. v6 v6 v6 v6 qˆ1 = q + λnˆp, qˆ2 = q − λnˆp The previous free space and the obstacle region are derived v8 v8 v8 v8 qˆ = q + λnˆp, qˆ = q − λnˆp by ignoring the physical size of members and nodes, such as 1 2 the obstacle polygon shown in Figure 2a that is derived by where λ is the growing size, which is taken to be the sum of regarding node v1 as a point and member (v6, v8) as a line the radius of the node and the radius of the edge. Then, the segment without thickness. In practice, a node is a sphere four rays shown in Figure 9a can be easily derived and a member is a cylinder, so it is not guaranteed that qˆv6 − qv1 qˆv6 − qv1 the minimum distance among members (Eq. 10) is greater rˆ16 = 1 , rˆ16 = 2 1 v6 v1 2 v6 v1 than the diameter of an edge module (or the sum of the kqˆ1 − q k kqˆ2 − q k radius of a node and the radius of an edge module) even the qˆv8 − qv1 qˆv8 − qv1 rˆ18 = 1 , rˆ18 = 2 involved node is not inside its obstacle region. This difficulty 1 v8 v1 2 v8 v1 kqˆ1 − q k kqˆ2 − q k is overcome through increasing the derived obstacle region by considering the physical size of VTT components. In nˆm is the unit vector perpendicular to the edge (v6, v8), the obstacle region, every polygon is converted into a pointing to v1 and lying on the plane formed by v1, v6, and v6 v6 v8 v8 λ polyhedron. For example, for the VTT shown in Figure 2a, v8. Then qˆ1 , qˆ2 , qˆ1 , and qˆ2 are moved by distance 2 on 16 16 18 18 the blue polygon defined by node v1 and edge module nˆm along −rˆ1 , −rˆ2 , −rˆ1 , and −rˆ2 , respectively: (v , v ) is one obstacle polygon for node v , and this polygon 6 8 0 λ is converted into a polyhedron shown in Figure8. The v6 v6 16 q¯1 =q ˆ1 − 16 rˆ1 boundary of this obstacle polyhedron is composed of five 2 |nˆm · rˆ1 | polygons that can be obtained as follows: v6 v6 λ 16 q¯2 =q ˆ2 − 16 rˆ2 2 |nˆm · rˆ2 | v1 v6 The current locations of nodes v1, v6, and v8 are q , q , λ v8 v8 v8 18 and q respectively, and the unit vector normal to the plane q¯1 =q ˆ1 − 18 rˆ1 2 |nˆm · rˆ1 | formed by these three nodes is nˆp (−nˆp is also a normal unit λ vector but in opposite direction). As shown in Figure 9b, we v8 v8 18 q¯2 =q ˆ2 − 18 rˆ2 v6 v8 2 |nˆ · rˆ | first adjust q and q by moving them along nˆp and −nˆp m 2 8 Journal Title XX(X)

With all the information, we can encode the boundary of the polyhedron that is composed of five polygons shown in Figure 9a in the following

 v6 v6 16 16
P1 = V = (¯q1 , q¯2 ) ,R = rˆ1 , rˆ2  v8 v8 18 18
P2 = V = (¯q1 , q¯2 ) ,R = rˆ1 , rˆ2  v6 v8 16 18
P3 = V = (¯q1 , q¯1 ) ,R = rˆ1 , rˆ1  v6 v8 16 18
P4 = V = (¯q2 , q¯2 ) ,R = rˆ2 , rˆ2 v6 v6 v8 v8 P5 = {V = (¯q1 , q¯2 , q¯1 , q¯2 )}

Note that P5 consists of no rays. v0 v0 Figure 10. Cfree(q ) with physical size of components being We can convert every obstacle polygon in Figure 2b considered. into an obstacle polyhedron (bounded by five polygons) in this way, and then process these polygons by Polygon Intersection and Boundary Search (Liu et al. 2020) to derive v0 v0 Cfree(q ) shown in Figure 10. The Boundary Search step can be simplified and the modified boundary search algorithm is shown in Algorithm1. Recall that the free space of a node is usually partitioned by its obstacle region into multiple enclosed subspaces, and given an obstacle polygon Ps and the set of all obstacle polygons Pobs generated by Polygon Intersection step, this algorithm can find the enclosed subspace with Ps being part of the boundary. Details of Polygon Intersection is introduced by Liu et al.(2020), (a) (b) and note that after Polygon Intersection, each polygon in v Figure 11. (a) Cb 0 (qv0 ) is computed with all members Pobs can only bound one enclosed subspace because every free v Cv1 (qv1 ) obstacle polygon is the boundary between the free space controlling 1 ignored. (b) bfree is computed with all v and the obstacle region. If we want to find the boundary of members controlling 0 ignored. v0 v0 Cfree(q ), we can set Ps to be the obstacle polygon that is v0 closest to q . In the algorithm, Si is the set of all edges of this node cannot traverse this plane, or it will be in a singular polygon Pi, sij is the jth edge of Pi, and Nij is the set of all configuration. In such a case, we also add this plane to polygons that share sij with Pi. For each obstacle polygon the obstacle polygon set when running the boundary search Pi, its normal vector pointing outwards the obstacle region algorithm. By taking these constraints into consideration, we is computed, and P ij in Line 8 is the innermost polygon in v0 v0 v1 v1 can derive Cbfree(q ) and Cbfree(q ) for the group containing Nij along the normal vector of Pi. v0 and v1 in the VTT shown in Figure6 and the result v0 v0 The new Cfree(q ) is smaller and bounded by more is shown in Figure 11. Compared with the space shown polygons. In addition to considering the physical size of the v0 v0 in Figure7, C (q ) becomes smaller and v0 cannot go robot components, for a single node, we can easily find the free beyond the plane formed by v2, v3, v6, and v7 due to the region when it is in a singular configuration. For a single singularity constraint. In the rest of the paper, we will use the node, if all of its neighbor nodes are on the same plane, then free space ignoring the physical sizes of robot components for illustration purposes. Algorithm 1: Boundary Search Algorithm 5.2 Path Planning for a Group of Nodes Input: Ps, Pobs Output: A set of boundary polygons Pb If there is only one node v in the group and the motion task v 1 Pb ← ∅; is to move the node from its initial position qi to its goal v v v v v v v 2 QP ← ∅; position qg where qi ∈ Cfree(qi ) and qg ∈ Cfree(qi ), then it v v 3 QP .enqueue(Ps); is straightforward to apply RRT approach in Cfree(qi ) and 4 while QP 6= ∅ do no collision can happen as long as the motion of each step is v v 5 Pi ← QP .dequeue(); inside Cfree(qi ) since this space is usually not convex. 6 Pb ← Pb ∪ {Pi}; When moving two nodes vi and vj in a group, sampling vi vi vj vj 7 for sij ∈ Si do will only happen inside Cbfree(q ) and Cbfree(q ) for vi and 8 P ij ← the innermost polygon in Nij; vj respectively. If there is no edge module connecting vi and V 9 if P ij ∈/ Pb P ij ∈/ QP then vj, then when applying RRT approach, the collision between 10 QP .enqueue(P ij); moving members and fixed members can be ignored as long vi vi 11 end as the motion of both nodes in each step are inside Cbfree(q ) vj vj 12 end and Cbfree(q ) respectively. Only self-collision inside the vi vj 13 end group — the collision among members in E ∪ E — e = 14 return Pb needs to be considered. If there is an edge module (vi, vj) which connects vi and vj, since this case is not Liu et al. 9

a discrete motion also needs to satisfy the length constraint (Eq.9), the angle constraint (Eq. 11), the manipulability constraint (Eq. 12), and the stability constraint when interacting with the environment. For these constraints, it is straightforward to discretize the motion in the current step to some resolution and check the states for validity at this resolution. When checking the stability constraint, we first find all supporting nodes by checking the height (a) (b) of every node, and if its height is close enough to the Figure 12. Light green triangle (N) is sweeped by member e ground, then this node is regarded as a supporting node. when moving node v to new location v0 along the yellow (→) There have to be at least three supporting nodes all the time. trajectory and the new position of member e is e0. e doesn’t Otherwise, this VTT cannot be stable. Then the center of collide with any other members in (a) but does collide with two mass of the current truss is computed and projected onto members in (b) during the motion (Liu and Yim 2019). the ground. Given this projected center of mass and all supporting nodes on the ground, the convex hull of them (the included in our obstacle model when computing the obstacle smallest convex polygon that contains all these points) can region for the group, it is also necessary to check the collision be computed efficiently using either Jarvis’s march (Jarvis vi between e = (vi, vj) and every edge module in E \ (E ∪ 1973) or Graham’s scan (Graham 1972). If the projected Evj ). center of mass is not on the boundary of the convex hull, In summary, when planning node vi and vj in a VTT then it is inside the support polygon that is the convex hull. G = (V,E), for each step, in order to avoid collision, it is Otherwise, this VTT cannot be stable. required to ensure the following With all these works, we can efficiently check the state validity and motion validity, and RRT-type approaches can vi vi • The motion of both node vi and vj are inside Cbfree(q ) be applied easily. Open Motion Planning Library (OMPL) vj vj and Cbfree(q ) respectively; by Sucan et al.(2012) is used to implement RRT for this v • No collision happens among edge modules in E i ∪ path planning problem. Evj ; • No collision happens between edge module e = 5.3 Geometry Reconfiguration Planning vi vj (vi, vj) and every member in E \ (E ∪ E ) if e = (vi, vj) exists. Assuming there are n nodes {vt ∈ V |t = 1, 2 ··· , n} that should be moved from their initial positions It is difficult to check the second and third collision cases v1 v2 vn v1 v2 vn qi , qi , ··· , qi to their goal positions qg , qg , ··· , qg during the motion if both nodes are moving simultaneously. respectively, we first divide these nodes into dn/2e groups. But since the step size for each node is limited and both Each group contains at most two nodes. Then the motion of them are moving in straight lines, we can first check the task is achieved by moving nodes one group by one group. collision during the motion of vi while keeping vj fixed, and Then this geometry reconfiguration problem results in then check the motion of vj. By doing so, the collision can finding a sequence of groups that can achieve the task. be checked efficiently using the approach presented by Liu If failed, try another grouping and find the corresponding and Yim(2019). Every edge module can be modeled as a sequence. Repeat this process until the task is finished, line segment in space, thus, when moving node v, every v otherwise failed. With this approach, we can solve this e ∈ E sweeps a triangle area, and if this member collides geometry reconfiguration planning problem much faster with another member e¯ ∈ E, then e¯ must intersect with the than the approach by Liu et al.(2020) and the detailed test triangle generated by e (Figure 12). Similar to the approach results are in Section8. in Section 5.1, we can buffer this triangle area in order to consider the size of physical components. Another way is to compute the local obstacle region of vi by only taking 6 Topology Reconfiguration vj E into account when moving vi, and similarly compute Topology reconfiguration involves changing the connectivity vi the local obstacle region of vj by only taking E into among edge modules and there are two atomic actions: account when moving vj. The local obstacle region of each Split and Merge. The undocking and docking process node can be derived easily. For vi, given NG(vi) that is is difficult for modular robotic systems, but sometimes vj the neighbors of vi and E , the local obstacle region of vi necessary for some motion tasks. We need to verify whether is simply the union of all the obstacle polyhedron defined the geometry reconfiguration process is enough or topology vj by all (v, e) ∈ NG(vi) × E . If the trajectory of vi that reconfiguration actions are needed. Recall that the free space is a line segment intersects with its local obstacle region, of a node is usually not a single connected component and if then collision happens. Repeat the same procedures when a motion task has initial and goal configurations in separated moving vj. If edge module (vi, vj) exists, when moving vi, enclosed subspaces, topology reconfiguration actions are we also need to consider the obstacle polyhedron defined needed. vi vj by vj and e ∈ E \ (E ∪ E ), and similarly consider the obstacle polyhedron defined by v and e ∈ E \ (Evi ∪ Evj ) i 6.1 Enclosed Subspace in Free Space when moving vj. In addition to this constraint that the distance between As mentioned before, each polygon after Polygon Inter- v every pair of members has to be greater than some value, section in Cobs is bounding only one enclosed subspace. 10 Journal Title XX(X)

Algorithm 2: Enclosed Subspace Search Input: VTT G = (V,E), node v ∈ V v Output: Set of all enclosed subspaces Cfree 1 Compute Pobs; 2 Ps ←polygon closest to node v; v v 3 Cfree(q ) ← BoundarySearch(Ps, Pobs); v v v 4 Cfree ← {Cfree(q )}; v 5 for Pi ∈ Pobs do v 6 if Pi ∈/ C, ∀C ∈ C then free (a) (b) 7 Cnew ← BoundarySearch(Pi, Pobs); v v 8 C ← C ∪ {C }; v0 v0 free free new Figure 14. (a) Enclosed subspace Cfree(q ) when v0 and v1 v0 v0 9 end are separated; (b) Enclosed subspace Cfree(q ) after merging 10 end v1 with v0. v 11 return Cfree

formed by v2, v3, v6, and v7. If merging v1 and v0 as v0 v0 v0 shown in Figure 14b, Cfree(q ) will be changed. The boundary originally blocked by members attached with node v1 disappears because all members controlling v0 and v1 are combined into a single set. In addition, this Merge action increases the motion manipulability of node v0 that can move through the square formed by v2, v3, v6, and v7. However, the side effect is that some originally reachable locations become not reachable, such as the space above the truss and v0 around the member (v4, v8). This is because E contains (a) (b) more members leading to the increase of the obstacle region of node v0. Figure 13. Cv0 (qv0 ) (a) Enclosed subspace free contains the When executing a Split action on a node, the basic current position of v0. (b) Another enclosed subspace is v0 v requirement is that this node has to be constructed by at separated from C (q 0 ) by obstacles. free least six edge modules. In the process, we need to physically split a node into a pair and move them away. In order Therefore we can compute all the enclosed subspaces by to guarantee motion controllability, singularity should be repeatedly applying Algorithm1 as shown in Algorithm2. avoided during the whole process. In some situations, we In this algorithm, we first obtain the set of all obstacle need to carefully consider if it is feasible to execute this v action. For example, when node v is on the same plane polygons Pobs by Polygon Intersection. Then, search for the 0 enclosed subspace containing the current node configuration with node v2, v3, v6, and v7 shown in Figure 15a, it is not v v allowed to split node v in the way shown in Figure 15b, — Cfree(q ) — from a starting polygon Ps, which is the 0 nearest one to the node (Liu et al. 2020). Afterward, we because one of the newly generated nodes (v0 in this case) v will be in a singular configuration. In comparison, if node v compute all other enclosed subspaces in Cfree by searching 0 the boundary starting from any polygon that has not been is outside the truss shown in Figure 16a, then it is feasible used. Figure 13 shows two enclosed subspaces of node v0 in to apply the same Split action to generate two new nodes a simple cubic truss. In total, there are 33 enclosed subspaces shown in Figure 16b. This constraint also applies for Merge. v0 In Figure 15b, since node v is in singular configuration, it in Cfree above the ground. 0 is not allowed to merge v1 with v0 to become a new truss 6.2 Topology Reconfiguration Actions There are two topology reconfiguration actions: Split and Merge. A node constructed by six or more edge modules can be split into two separate nodes, and two separate nodes can merge into an individual node. These actions can significantly affect the motion of the involved nodes and there are several constraints when applying them. For v0 v0 the VTT shown in Figure6, Cfree(q ) that is the enclosed subspace containing the current location of node v0 is shown in Figure 14a. This node can move to some locations outside truss but its motion is also blocked in some directions. The (a) (b) members attached with node v1 blocked the motion of node v0 on this side, and the plane formed by node v2, v3, v6, and Figure 15. (a) Node v0, v2, v3, v6, and v7 are on the same v0 v7 also blocks the motion of v0 due to: 1. collision avoidance plane. (b) Splitting node v0 in this way to separate E into two with member (v2, v3), (v2, v6), (v3, v7), and (v6, v7); 2. sets is not valid, because node v0 will be in singular configuration. Similarly, it is not valid to merge v1 with v0. singularity avoidance that v0 cannot move through the square Liu et al. 11

of all possible ways to separate Ev into two groups. If v is 0 00 split into v0 and v00, then Ev is separated into Ev and Ev 0 00 accordingly. This split process is denoted as (Ev ,Ev ). Not all possible ways in A can be applied on node v. Given a valid VTT G = (V,E) and a node v, a Split action can v0 v00 v0 v00 0 00 be encoded as a tuple (E ,E , qs , qs ) where v and v v0 v00 are the newly generated node after splitting, and qs and qs are the locations of these two new nodes. For simplicity, v0 v 00 0 qs = q and v is moved away from v along a unit vector (a) (b) dv so that there is no collision introduced after this process. v0 The direction of dv is determined by qs (the current location Figure 16. (a) Node v0 is outside the truss. (b) It is possible to 0 0 0 of v ) and NG(v ) (the neighbors of v ), and can be derived split node v0 in this way to generate v1. Reversely, v0 and v1 by normalizing the following vector can be merged. 0 X qv − qvˆ s (15) kqv0 − qvˆk 0 s vˆ∈NG(v )

and the distance moved along vector dv can be tried iteratively until there is no collision in this VTT. In addition to the collision-free requirement, the resulted VTT should satisfy all other hardware constraints described in Section 4.2 0 00 after splitting, and let VC = {v , v } when checking the motion manipulability by Eq. 12. Given a location of node v and one way to split Ev into (a) (b) 0 00 Ev and Ev , Function ComputeSplitAction is used to check Split Figure 17. Split node v0 into v0 and v1: (a) a right way to move and compute this action. In this function, G is the node v1 away from node v0; (b) a wrong way to move node v1 given VTT, a ∈ A is a possible split way, and D, ∆D, away from node v0. and Dmax are three parameters. Function VTTValidation returns true if a given VTT satisfies all hardware constraints, otherwise returns false. Function VTTCollisionCheck shown in Figure 15a. However, it is feasible to merge v0 and returns false if there is no collision among all members, v1 in Figure 16b to become v0 in Figure 16a. otherwise returns true. The basic idea is to move v00 gradually After splitting a node, the members attached to this away from v0 until a valid VTT is found. The maximum 0 00 node are separated into two groups that can be controlled distance between v and v for searching is Dmax that should independently. However, we need to consider how to move be a small value. Within this small range, member collision the two newly generate nodes away from each other. In Figure 17a, after splitting v0 into v0 and v1, v1 is moved v to the right side of v0 and this motion can separate Function ComputeSplitAction(G, q , a ∈ A) these involved members without any collision. However, in Data: D, ∆D, Dmax v Figure 17b, for the same Split action, v1 is moved to 1 Move node v to q ; the left side of v0 and this motion is in fact not feasible 2 if VTTValidation(G) = False then because members will collide. This is also important for 3 return Null; Merge. When merging two nodes, we first need to move 4 end 0 00 them to some locations that are close to each other. If the 5 Split node v into v and v according to a ∈ A; new locations are not selected correctly, it is impossible to v0 v v00 v 6 qs ← q , qs ← q ; move them there and execute Merge action. 7 Compute dv; 8 repeat 6.3 Topology Reconfiguration Planning v00 v 9 qs = q + Ddv; 00 v00 Given a VTT G = (V,E) and the motion task that is to 10 Move node v to q ; v v v v move node v from qi to qg , if qi and qg belong to the same 11 if VTTCollisionCheck (G) = True then enclosed subspace, then geometry reconfiguration planning 12 D ← (D + ∆D)dv; is able to handle this problem by either the approach by Liu 13 else et al.(2020) or the approach introduced in Section 5.2. 14 if VTTValidation(G) = True then v0 v00 v0 v00 Otherwise, topology reconfiguration is needed. The node has 15 return (E ,E , qs , qs ); to execute a sequence of Split and Merge in order to avoid 16 else collision with other members. 17 return Null; There are multiple ways for a node v to split into nodes 18 end 0 00 v and v as there are multiple ways to take the members 19 end into two groups, and it is straightforward to compute all 20 until D ≤ Dmax; v possible ways to split E into two groups in which both 21 return Null; sets contain at least three edge modules. Let A be the set 12 Journal Title XX(X) is a more significant constraint, such as the case shown in Algorithm 3: Sample Generation Figure 17b. Hence initially only checking collision among Input: tCv , G, A members, and once a collision-free location for v00 is found, free Output: S, AS check other hardware constraints. Reversely, if merging v0 00 v 1 S ← ∅; and v as v at a location q , with this computed Split S v0 v00 v0 v00 v0 v 2 Initialize an empty map A ; action (E ,E , qs , qs ), we can first check if qs = q ∈ v0 v0 v00 v00 v00 0 v0 3 Initialize dmin; Cfree(q ) and qs ∈ Cfree(q ), and if true, move v to qs t v 00 4 N ← SampleNumber( C ); and move v00 to qv , and then merge them at qv. max free s 5 for k = 1 to K do v v Given q , the current position of node v, and Cfree = 6 Avalid ← ∅;; { tCv |t = 1, 2, ··· ,T } that contains T enclosed subspaces t v free 7 qrand ← RandomPosition( Cfree); in the free space of node v, apply a valid Split action on 8 foreach a ∈ A do v v0 v00 this node to separate E into E and E and generate 9 aˆ = ComputeSplitAction(G, q, a); 0 00 v0 v00 two new nodes v and v . q and q are the locations of 10 if aˆ is not Null then 0 00 v0 v0 v00 v00 v and v , and Cfree(q ) and Cfree(q ) can be computed 11 Avalid = Avalid ∪ {aˆ}; t v accordingly. Assuming there is a position q ∈ Cfree where 12 end node v can also be split in the same way (Ev can be separated 0 00 0 00 0 00 13 end into Ev and E with Split action (Ev ,Ev , qv , qv )), V 0 0 0 00 00 00 s s 14 Sclose ← {q|q ∈ S kq − qrandk ≤ dmin}; qv ∈ Cv (qv ) qv ∈ Cv (qv ) v0 v00 V if s free and s free , namely and 15 if Sclose = ∅ |S| < Nmax then v0 v00 can navigate to qs and qs respectively and merge at q, 16 S ← S ∪ {qrand}; v v t v S then this node v can navigate from Cfree(q ) to Cfree by a 17 A [q] = Avalid; pair of Split and Merge actions, and these two enclosed V 18 else if Sclose 6= ∅ |S| < Nmax then subspaces can be connected under this action pair. This is 19 if |Avalid| = |A| then the transition model when applying topology reconfiguration 20 foreach q ∈ Sclose do S actions. From Section 6.2, it is shown that the possible ways 21 del A [q]; to split a node are highly dependent on the location of 22 end the node, and the resulted enclosed subspaces of the newly 23 S ← S \ Sclose + {qrand}; generated nodes can be very different when splitting the node S 24 A [qrand] = Avalid; at different locations. Hence, there are two phases in the 25 else topology reconfiguration planning: sample generation and 26 UpdateFlag ← False; graph search. 27 foreach q ∈ Sclose do S In sample generation, multiple samples are generated 28 if A [q] ⊂ Avalid then S for every enclosed subspace. These samples are expected 29 del A [q]; to provide valid Split actions as many as possible and 30 S ← S \ {q}; also cover the space as much as possible. We introduce 31 Nmax ← Nmax − 1; Algorithm3 to sample a given enclosed subspace. S is the 32 UpdateFlag ← True; AS S set containing all generated valid samples and stores 33 else if Avalid \A [q] 6= ∅ then the Split actions for every sample. Nmax is the maximum 34 UpdateFlag ← True; t v number of samples for a given enclosed subspace Cfree and 35 end it is determined by the size of the space: a larger space is 36 end expected to have more samples. In our setup, we find the 37 if UpdateFlag = True then x y z range of a given space along , , and axis denoted as 38 S ← S + {q}; xrange, yrange, and zrange respectively, then Nmax is simply S l . m 39 A [qrand] = Avalid; q 2 2 2 xrange + yrange + zrange dmin in which dmin is the 40 Nmax ← Nmax + 1; minimum distance between every pair of samples. K is 41 end the maximum number of iterations set by users and can 42 end be related to Nmax. For each iteration, we first randomly 43 end generate a sample qrand that is inside the given enclosed 44 end subspace, then find all valid Split actions stored in Avalid (Line 6 — 13). Then find all previous valid samples that are close to qrand (Line 14). If qrand is far from all previous Split valid samples and the size of S is less than Nmax, add actions compared with all valid close samples. If the this new sample and store its valid Split actions (Line first condition is true, remove the old sample and add the 15—17). If qrand is close to some previous valid samples, new sample to S. If the second condition is true, add the there are two cases for consideration. If this new sample new sample to S (Line: 26—41). We run Algorithm3 for provides all possible Split actions, then remove all valid every enclosed subspace to generate enough samples, and v close samples and only keep this new sample for this area then add qi (the initial location of node v) to the sample set v v v since this is the best option (Line 19—24). Otherwise, we of Cfree(qi ) and add qg (the goal location of node v) to the v v C check two conditions: 1. whether there exists any valid close sample set of Cfree(qg ). After sample generation phase, S sample that has fewer number of valid Split actions than and the corresponding Split actions for all samples CAS v qrand; 2. whether the new sample qrand introduces any knew are obtained for every enclosed subspace C ∈ Cfree. Liu et al. 13

Algorithm 4: Topology Reconfiguration Planning v v v C C S v Input: G, qi , qg , Cfree, {( S, A )|C ∈ Cfree} Output: Tree of enclosed subspaces p v v v v 1 if Cfree(qi ) = Cfree(qg ) then 2 return Null; 3 end  v v v v 4 Q ← Cfree(qi ), Cfree(qg ) , Q ← ∅; v v v v 5 g(Cfree(qi )) ← 0, g(Cfree(qg )) ← ∞; v v 6 while Cfree(qg ) ∈ Q do Figure 18. Topology connections among three enclosed 7 C ← arg min g(C); C∈Q subspaces of node v. 8 Q ← Q \ {C}; 9 Q ← Q ∪ {C}; v V Then we can enter the graph search phase to generate 10 foreach C ∈ Cfree \{C} C ∈/ Q do C C C C a sequence of topology reconfiguration actions. With the 11 foreach ( q, q) ∈ S × S do C C transition model discussed before, a graph search algorithm 12 if ValidMotion( q, q) = True then can be applied to compute a sequence of enclosed subspaces 13 if C ∈ Q then v v v v C C starting from Cfree(qi ) to Cfree(qg ) while exploring the 14 if g(C) + c( q, q) < g(C) then topology connections among these enclosed subspaces. An C C 15 g(C) ← g(C) + c( q, q); example is shown in Figure 18. Here, the graph has enclosed 16 p(C) ← C; subspaces as vertices. An edge in this graph connecting two enclosed subspaces denotes that node v can move from a 17 end sample in one enclosed subspace to a sample in the other. 18 else v v 19 Q ← Q + {C}; The graph is built from Cfree(q ), grows as valid transitions C C among enclosed subspaces are found, and stops when the 20 g(C) ← g(C) + c( q, q); v enclosed subspace containing qg is visited. A graph search 21 p(C) ← C; algorithm designed based on Dijkstra’s framework is shown 22 end in Algorithm4. 23 end v v Line 1 — 5: If qi and qg are in the same enclosed subspace, 24 break; then no topology reconfiguration is needed. Otherwise, we 25 end Q Q Q make two sets and where contains all newly checked 26 end or non-visited enclosed subspaces and Q contains all visited 27 end enclosed subspaces. The size of these two sets will change v as the algorithm explores Cfree. Initially, only the enclosed v v v subspace containing qi that is Cfree(qi ) and the enclosed v v v and the parent of this newly checked enclosed subspace, and subspace containing qg that is Cfree(qg ) are in Q, and v v update set Q. There can be multiple ways to transit between the algorithm starts with Cfree(qi ). The value g(C) is the g two enclosed subspaces. For example, there are two edges cost of the path from qi to the enclosed subspace C, so v v v v between t2 Cv and t3 Cv shown in Figure 18. In our setup, g(Cfree(qi )) = 0 and g(Cfree(qg )) = ∞ in the beginning. free free Line 7 — 9: Every iteration starts with the enclosed we just keep the first one being found. Since CS and CS are subspace that has the lowest cost g(C) in Q. At the beginning, randomly generated, it is possible that the condition in Line v v C C Cfree(qi ) has the lowest cost. After selecting an enclosed 12 is failed although there does exist q and q that can pass subspace, update Q and Q. this condition. v v Line 10 — 26: Iterate every enclosed subspace C except Once Cfree(qg ) is visited, the algorithm ends. With p, v C in Cfree and check if it is already visited. If so, then this a tree with visited enclosed subspaces as vertices, it is v v potential transition is not a new transition. Otherwise, check straightforward to find the optimal path connecting Cfree(qi ) v v if there exists a valid motion to move the node from any and Cfree(qg ) as well as all the samples the node need to sample in C that is the enclosed subspace with the lowest traverse. For example, in Figure 18, when node v traverses t1 v t3 v t1 v cost to any sample in C. If this is true, then there are two from q1 to q1 , it first moves to q2 , then Split and t3 v t3 v cases: this subspace is not checked for the first time namely Merge at q2 , and finally move to q1 . Moving a node that there is already a connection between this enclosed inside one of its enclosed subspaces can be solved easily subspace and another enclosed subspace, or this subspace has by geometry reconfiguration planning. After splitting the never been checked which means they have no connection node into a pair, we can also apply geometry reconfiguration before. For the first case, we need to check whether its cost planning to move them to the computed positions in the next needs to be updated. c(Cq, Cq) is the cost of the motion enclosed subspace for merging. from Cq to Cq. This cost can be related to the estimated distance or other factors. In our setup, all valid motions from 7 Locomotion one enclosed subspace to another one have the same cost. If its cost is updated, then its parent p(C) should also be Unlike the previous control for reconfiguration, the center updated accordingly. For the second case, initialize the cost of mass during locomotion moves over a large range by 14 Journal Title XX(X)

(a) (b) (c)

Figure 19. A VTT in octahedron configuration executes a single rolling locomotion step. (a) Initially node v0, v1, and v2 forms the support polygon shown in blue, and the center of mass projected onto the ground (green dot) is within this support polygon. The truss wants to roll from its current support polygon to an adjacent support polygon formed by node v1, v2, and the new tipping location. (b) v3 and v5 are moved so that the support polygon is expanded and the center of mass projected onto the ground is on the member (v1, v2). (c) v0 and v4 are moved to the destination to finish this locomotion step, and the center of mass projecte onto the ground is within the new support polygon formed by node v1, v2, and v3. continuously interacting with the environment. In a rolling is equivalent to rotating the truss with respect to an edge of G locomotion step, a VTT rolls from one support polygon to this facet e ∈ Ef until the other incident facet of this edge an adjacent support polygon (Figure 19a). In this process, it f e becomes the new support polygon. An example of this is useful to maintain statically stable locomotion to prevent process is shown in Figure 19. Initially facet f = (v0, v1, v2) the system from receiving impacts from the ground as is the support polygon, and the other incident facet of edge G e repeated impacts may damage the reconfiguration nodes. e = (v1, v2) ∈ Ef is f = (v1, v2, v3). If we want to control Due to this requirement, a rolling locomotion step has to the robot to do locomotion from Figure 19a to Figure 19c, be manually divided into several phases to have control the result of this process is equivalent to rotating the truss over the center of mass (Park et al. 2020; Usevitch et al. with respect the fixed edge e = (v1, v2) until the support 2020). Our approach can deal with this issue automatically polygon becomes f e. This means the input command for while planning the motion of nodes that need to be moved. each locomotion step can be simply an edge of the current A high-level path planner can generate a support polygon support polygon, e.g. (v1, v2) for the scenario in Figure 19. trajectory given an environment and a locomotion task, and the locomotion planner can ensure that the truss can follow 7.2 Locomotion Planning this support polygon trajectory without violating constraints Given a VTT G = (V,E), its truss polyhedron model P G = and receiving impacts from the ground. (EG,F G) and its current support polygon f ∈ F G can be derived. Given a locomotion command e ∈ f, we can find e G 7.1 Truss Polyhedron f and compute its normal vector nf e pointing inwards P The boundary representation of a VTT is modeled as a (e.g. vector n for facet (v1, v2, v3) in Figure 19a), then the convex polyhedron which can be either defined initially or rotation angle α between nf e and the normal vector of the | obtained from the set of node positions by computing the ground [0, 0, 1] is simply convex hull of them using existing computational geometry  |  nf e • [0, 0, 1] algorithms, such as Quickhull (Barber et al. 1996). Given α = arccos (16) a VTT G = (V,E), its polyhedron representation can be knf e k G G G G fully defined as P = (E ,F ) in which F is the set e e G The rotation axis is along edge e = (v1, v2) and its direction of facets forming the boundary of this VTT and E is can be easily determined by the right-hand rule. For the the set of edges forming all facets, namely each facet is G example shown in Figure 19, the rotation axis is simply composed of multiple edge members in E and each edge qv1 −qv2 v v , namely the unit vector pointing from node v2 to of EG is incident to two facets. For example, the boundary kq 1 −q 2 k node v . Assume the rotation axis is pointing from ve to representation of the VTT in Figure 19a is an octahedron 1 1 ve, then the rotation angle and the rotation axis determine in which edge (v , v ) is incident to facet (v , v , v ) and 2 1 2 1 2 3 a rotation matrix Re, and for every node v ∈ V \{v , v } facet (v , v , v ). In this example, all edges are contained 1 2 0 1 2 during this locomotion process, its initial position qv is in its polyhedron representation. However, for the VTT in i qv qv v0 known as and its goal position g can be derived as Figure 13, v0 and its corresponding edges in E are not involved in its polyhedron representation. e e v e v v1 v1 G G qg = R (q − q ) + q (17) Suppose the set of edges forming a facet f ∈ F is Ef ⊂ G G E , and ∀e ∈ Ef , the other incident facet of edge e is Then we can make use of the approach presented in f e ∈ F G. Every facet can be a support polygon as long as Section 5.3 to plan the motion for all of these involved nodes all of its vertices are on the ground. Assume f ∈ F G is the to execute the locomotion step. Due to the stability constraint current support polygon and a single rolling locomotion step that at least three nodes need to contact the ground to form Liu et al. 15 a support polygon and the center of mass represented on the and with different parameters to show the universality of our ground has to be within this support polygon, the planner framework. All tests run on a laptop computer (Intel Core will automatically move nodes first to expand the support i7-8750H CPU, 16GB RAM) and the workspace is a cuboid. polygon so that the center of mass can be moved in a larger range. An example of this non-impact rolling locomotion 8.1 Geometry Reconfiguration planning result is shown in Figure 19. In this task, the initial The geometry reconfiguration planning test changes the cube support polygon is facet (v , v , v ) and we want to roll 0 1 2 shape of a VTT, Figure 20a, to a tower shape, Figure 20b. the truss so that facet (v , v , v ) becomes the new support 1 2 3 This VTT is composed of 21 members with initial positions polygon. The locomotion command is simply (v , v ), the 1 2 of nodes listed in the following rotation axis and the rotation angle can be computed as mentioned, and four nodes (v , v , v , and v ) have to 0 3 4 5 qv0 = [−1.605, −0.771, 2.075]| move to new locations which can be derived from Eq. (17). qv1 = [0.7779, −0.7642, 2.075]| Because of the stability constraint, node v cannot be moved 0 qv2 = [−0.4756, −2.022, 0.075]| at the beginning, or the robot won’t be stable since there will qv3 = [−0.4142, 0.4228, 2.175]| be no support polygon. The planner chooses to move v and 3 qv4 = [−1.605, −0.771, 0.075]| v first to expand the support polygon which is formed by 5 qv5 = [0.3819, −0.3707, 0.125]| node v , v , v , and v , and the center of mass is moved 0 1 2 3 qv6 = [−0.4314, −0.9559, 1.2321]| towards the target support polygon (Figure 19b). The center qv7 = [−0.4756, −2.022, 2.075]| of mass projected onto the ground is always within the initial qv8 = [0.1819, −0.1707, 0.075]| support polygon. After the support polygon is expanded, v0 and v4 are moved, and, in the meantime, the projected center The constraints for this task are Lmin = 1.0 m, Lmax = of mass enters the target support polygon (Figure 19c). ¯ 3.5 m, θmin = 0.3 rad, and µ¯min = 0.1. Four nodes v1, v3, v5, and v6 are involved 8 Test Scenarios in this motion task and their goal positions in v1 | the tower VTT are qg = [0.18, −0.17, 4.13] , The motion planning framework is implemented in C++. v3 | v5 | qg = [−1.61, −0.77, 4.08] , qg = [−0.48, −2.02, 4.08] , Four example scenarios were conducted to measure the and qv6 = [0.18, −0.17, 2.13]|. These four nodes are effectiveness of our approach. The performance of the g separated into two groups {v3, v5} and {v1, v6} which reconfiguration framework is compared with the approach v v is randomly selected. We first compute Cb 3 (q 3 ) and by Jeong et al.(2018) and Liu and Yim(2019). The free i Cv5 (qv5 ) performance of the locomotion framework is compared with bfree i , and then do planning for these two nodes. The the approach by Park et al.(2020) and Usevitch et al.(2020). motion of node v3 and v5 is shown in Figure 21. Most These experiments are also tested under different constraints of the obstacle region in this step is surrounded by the v3 v3 v5 v5 subspace Cbfree(qi ) and Cbfree(qi ), hence it is easier for them to extend outwards first in order to navigate to the goal positions. This motion process moves the projected center of mass towards one edge of the support polygon but the planner can constrain the projected center of mass within the support polygon. After planning for v3 and v5, the v1 v1 v6 v6 truss is updated, and Cbfree(qi ) and Cbfree(qi ) are computed accordingly. Finally the planning for v1 and v6 finishes this motion task with the result shown in Figure 22. For v1 and v in this updated truss, the enclosed subspace Cv1 (qv1 ) and (a) (b) 6 bfree i v6 v6 Cbfree(qi ) almost covers the whole workspace so it is also Figure 20. The motion task is to change the shape of a VTT easy to navigate to the goal positions. The minimum length from (a) a cubic truss for rolling locomotion to (b) a tower truss (Lmin) and the maximum length (Lmax) of all moving edge for shoring. modules, the minimum angle between every pair of edge

(a) (b) (c) (d) (e)

Figure 21. v3 and v5 firstly extend outwards, and then move upward to their goal positions. The support polygon is formed by three nodes (v2, v4, v5) on the ground shown as a triangle (N) and the green dot (•) is the center of mass represented on the ground. 16 Journal Title XX(X)

(a) (b) (c)

v1 v1 v6 v6 Figure 22. v1 and v6 can navigate to their goal positions easily since Cbfree(qi ) and Cbfree(qi ) almost cover the whole workspace.

2.0

(m) 1.5 min L 1.0 0 1000 2000 3000 Time Sequence

3.5 (a) (b)

(m) 3.0 Figure 24. (a) A VTT is constructed from 18 edge modules with 6 nodes. (b) The goal is to move node v5 from its initial position max 2.5

L to a position inside the truss. 2.0 0 1000 2000 3000 Time Sequence 0.7

0.5 (radian) min

θ 0.3 0 1000 2000 3000 Time Sequence

0.40

µ 0.25

0.10 Figure 25. Cv5 (qv5 ) is the yellow space on the upper left and 0 1000 2000 3000 free i Cv5 (qv5 ) Time Sequence free g is the green space on the lower right. They are not connected and separated by the obstacle region generated from edge (v3, v4). Figure 23. The minimum length (Lmin) and the maximum length (Lmax) of all moving edge modules, the minimum angle θ between every pair of edge modules ( min), and the motion trials and the mean running time is 4.158 s with a standard manipulability (µ) are measured throughout the geometry reconfiguration process in Figure 21 and Figure 22. deviation of 1.952 s and the success rate is 100%. 8.2 Topology Reconfiguration 8.2.1 Scenario 1 The VTT configuration used for this topology reconfiguration example is shown in Figure 24a modules (θ ), and the motion manipulability (µ) are shown min with the following nodes’ positions in Figure 23. Note that Lmin, Lmax, and µ are not necessarily to be continuous because nodes that are under control are qv0 = [0.05, 0, 0075]| qv1 = [0.1, 1.8, 0.075]| changing. This motion task is also demonstrated by Jeong qv2 = [2.1, 1.9, 0.075]| qv3 = [2.1, 0, 0.075]| et al.(2018) using the retraction-based RRT algorithm. This qv4 = [0, 2.1, 3.225]| qv5 = [1.95, 0.9, 3]| algorithm cannot solve this motion planning task in 100 qv6 = [0, 0, 3.025]| trials unless an intermediate waypoint is manually specified to mitigate the narrow passage issue. The success rate for the The constraints for this task are Lmin = 1.0 m, Lmax = ¯ planning from the initial to the waypoint is 99% and 98% 5.0 m, θmin = 0.2 rad, and µ¯min = 0.1. from the waypoint to the goal. In comparison, our algorithm The motion task is to move v5 from its initial position v5 | v5 | doesn’t need any additional waypoints, and we did 1000 qi = [1.95, 0.9, 3] to a goal position qg = [1, 1.2, 0.9] Liu et al. 17

(a) (b) (c) (d) (e)

(f) (g) (h) (i) (j)

v5 v5 Figure 26. The sequence to move v5 from qi to qg is shown. (a) — (c) First move v5 to a new location. (d) Split v5 into a pair. (e) — (h) Move these two newly generated nodes in different directions to go around the edge module (v3, v4). (i) — (j) Merge them v5 into an individual node and move this node to qg .

2.0 (m) 1.5 min L 1.0 0 500 1000 1500 Time Sequence

5.0

(m) 3.5 (a) (b) max

L 2.0 Figure 28. (a) A VTT is constructed from 19 members with 9 v 0 500 1000 1500 nodes. (b) The task is to move 0 from its initial position to a Time Sequence position outside the cubic truss.

0.4

(radian) 0.3 min θ 0.2 0 500 1000 1500 Time Sequence

0.7

µ 0.4

0.1 0 500 1000 1500 Time Sequence

Figure 27. The minimum length (Lmin) and the maximum length (Lmax) of all moving edge modules, the minimum angle v v between every pair of edge modules (θmin), and the motion Figure 29. v has to move from Cfree(qi ) that is the yellow manipulability (µ) are measured throughout the topology enclosed subspace to the green enclosed subspace, and then v v reconfiguration process in Figure 26. Cfree(qg ) that is the blue enclosed subspace. 18 Journal Title XX(X)

(Figure 24b). This motion cannot be executed with only 53 enclosed subspaces and it takes on average only 43.090 s v5 v5 v5 v5 geometry reconfiguration because Cfree(qi ) and Cfree(qg ) to solve this motion task with a standard deviation of 4.424 s shown in Figure 25 are separated by the obstacle region in 1000 trails, including the enclosed subspace computation, generated from edge module (v3, v4). For this task, the topology reconfiguration planning and two-node geometry minimum distance between two nodes is dmin = 1.0 m and reconfiguration planning, and the success rate is 100%. N is constrained to be greater that or equal to 3, namely max 8.2.2 Scenario 2 Another motion task in which topology it is expected to have at least 3 samples for every enclosed reconfiguration actions are involved is shown in Figure 28 subspace, and the maximum number of iterations K = that is to move v from a position qv0 inside the cubic truss 5N . 0 i max (Figure 28a) to a new position qv0 (Figure 28b). The initial With our topology reconfiguration planning algorithm g positions of all nodes are (Algorithm4), one pair of Split and Merge actions is sufficient. v5 is moved to a new location , then split into a qv0 = [0, 0, 1.075]| qv1 = [1, 1, 0.075]| 0 00 v v pair of nodes (v5 and v5 ) so that both of these two newly q 2 = [−1, 1, 2.075]| q 3 = [−1, −1, 2.075]| v5 v 5 v4 | v5 | generated nodes can navigate to Cfree(qg ) and merge into q = [1, −1, 2.075] q = [1, 1, 0.075] a single node. Then the geometry motion planning is used qv6 = [−1, 1, 0.075]| qv7 = [−1, −1, 0.075]| 0 00 v to plan the motions of v5 and v5 and control them to the q 8 = [1, −1, 0.075]| target positions for merging. Finally, merge them back to v5 an individual node and then move the node to qg . The The constraints for this task are Lmin = 0.5 m, Lmax = ¯ detailed process is shown in Figure 26. The minimum length 5.0 m, θmin = 10°, and µ¯min = 0.1. v0 | v0 (Lmin) and the maximum length (Lmax) of all moving edge In this task, qg = [−0.64, −2.19, 2.78] . Similarly, qi v0 modules, the minimum angle between every pair of edge and qg are in two separated enclosed subspaces, and one modules (θmin), and the motion manipulability (µ) are shown solution is to apply topology reconfiguration actions twice v in Figure 27. to traverse three enclosed subspaces in Cfree shown in This motion task has been solved in Liu and Yim Figure 29. For this task, dmin = 0.5, Nmax is constrained to (2019) with the graph search algorithm exploring 8146 be greater than or equal to 3, and K = 8Nmax. VTT configurations with a more complex action model in The detailed planning result is shown in Figure 30. We order to find a valid sequence of motion actions. With the first move the node v0 to a location outside the cubic truss proposed framework, the free space of v5 is partitioned into (Figure 30a — Figure 30c), and then split it into two. If

(a) (b) (c) (d) (e)

(f) (g) (h) (i) (j)

(k) (l) (m) (n) (o)

v0 v0 v Figure 30. The sequence to move v0 from qi to qg by traversing three enclosed subspaces in Cfree is shown. (a) — (c) Move node v0 to traverse the plane formed by node v3, v4, v7, and v8. (d) Split the node into a pair here so that both newly generated nodes can move around edge module (v3, v7) while avoiding singular configuration. (e) — (h) Two newly generated nodes are moved to a location inside the green enclosed subspace and merge. (i) — (j) Move the merged node to a new location and split it in a different way to generate two new nodes. (k) — (n) One node traverses the space inside the cubic truss to go to the blue enclosed subspace, and the other node moves upwards. Then these two nodes merge at a location inside the blue enclosed subspace. (o) Move the node to the target location to finish the task. Liu et al. 19

3.5

(m) 2.0 min L 0.5 0 1000 2000 3000 4000 5000 Time Sequence

5.0

(m) 3.5 (a) (b) max L 2.0 Figure 32. The locomotion task is to roll the truss from (a) to 0 1000 2000 3000 4000 5000 (b). Time Sequence

0.80 internally. The initial locations of all nodes are (rad) 0.45 qv0 = [−0.7217, 0, 0.075]| min

θ v1 | 0.15 q = [0.3608, 0.6250, 0.075] 0 1000 2000 3000 4000 5000 qv2 = [0.3608, −0.6250, 0.075]| Time Sequence qv3 = [0.7217, 0, 1.0956]| 0.80 qv4 = [0.7217, 0, 1.0956]| qv5 = [−0.3608, −0.6250, 1.0956]|

µ 0.45 qv6 = [0, 0, 0.5853]|

0.10 0 1000 2000 3000 4000 5000 The constraints for this locomotion task are Lmin = 0.3 m, ¯ Time Sequence Lmax = 5.0 m, θmin = 0.3 rad, and µ¯min = 0.1. The task is to roll this VTT to an adjacent support polygon shown in Figure 31. L The minimum length ( min) and the maximum Figure 32b, and the locomotion command is (v1, v2). Five length (Lmax) of all moving edge modules, the minimum angle nodes (v , v , v , v , and v ) are involved in this process and θ 0 3 4 5 6 between every pair of edge modules ( min), and the motion their goal locations can be computed by Eq. (17). manipulability (µ) are measured throughout the topology reconfiguration process in Figure 30. The detailed locomotion process is shown in Figure 33. Five nodes are divided into three groups and the whole process is also divided into three phases automatically. In the first phase, v3 and v5 are moved to expand the the node is split inside the cubic truss, there is no way to support polygon (Figure 33c). Then v0 and v4 are moved merge them inside the green enclosed subspace (Figure 30g) gradually to move the center of mass towards the adjacent because one node has to traverse a region formed by node support polygon shown in Figure 33d. Once the center of mass represented on the ground is inside the target support v3, v4, v7, and v8 resulting in singular configurations. After this Split action, do geometry reconfiguration planning to polygon, v0 can be lifted off the ground and both v0 and control them to the green enclosed subspace and merge them v4 can be moved to their target locations (Figure 33e— back. Then move the node to a different location inside the Figure 33h). Finally, move node v6 to its target location green enclosed subspace (Figure 30i) and then split the node shown in Figure 33j to finish the whole process. in a different way (Figure 30j) in order to navigate these The minimum length (Lmin) and the maximum length v v0 (Lmax) of all moving edge modules, the minimum angle two nodes to Cfree(qg ) (Figure 30k — Figure 30m) and v0 between every pair of edge modules (θmin), and the motion merge them in Figure 30n. Eventually move the node to qg manipulability (µ) are shown in Figure 34. We randomly (Figure 30o). The minimum length (Lmin) and the maximum generate 1000 rolling commands and the average planning length (Lmax) of all moving edge modules, the minimum time is 8.297 s with the standard deviation being 5.935 s in angle between every pair of edge modules (θmin), and the motion manipulability (µ) are shown in Figure 31. The 1000 trails, and the success rate is 100%. In these tests, average planning time is 316.599 s with a standard deviation the maximum consuming time is 24.459 and the minimum being 18.258 s in 100 trails, and the success rate is 68%. In consuming time is 0.473 s. The locomotion of this VTT these tests, the maximum consuming time is 360.187 s and is also tested by Park et al.(2020) and the performance the minimum is 258.647 s. The search space is larger and comparison is shown in Table1. Our planner has better more samples are generated in order to find the sequence of performance in terms of both efficiency and robustness. topology reconfiguration actions which consumes more time. An octahedron rolling gait is computed based on an optimization approach by Usevitch et al.(2020). Impacts

Table 1. Comparison with Park et al.(2020). 8.3 Locomotion Avg. Plan Time Success Rate Optimization 18.834 s 38.3% (820 trials) The VTT used for the locomotion test is shown in Figure 32a Sampling-Based 8.297 s 100% (1000 trials) that is an octahedron with three additional edge modules 20 Journal Title XX(X)

(a) (b) (c) (d) (e)

(f) (g) (h) (i) (j)

Figure 33. The planned motion for this locomotion task. (a) — (c) Move node v3 and v5 to first expand the support polygon. (d) Move v0 and v4 to move the center of mass towards the target support polygon. (e) — (h) Once the center of mass represented on the ground is inside the target support polygon, lift v0 and move both v0 and v4 to their target locations. (i) — (j) Finally move v6 to its target location to finish the locomotion process.

1.3 Table 2. Comparison with Usevitch et al.(2020). Avg. Plan Time Success Rate (m) 0.8 Optimization 166 s Not provided min

L Sampling-Based 1.181 s 100% (1000 trials) 0.3 0 250 500 750 1000 1250 Time Sequence

2.3 in Table2. The solution (Figure 19) can be derived as fast as 1.181 s in average with the standard deviation being 1.033 s (m) 1.5 in 1000 trials, and the success rate is 100%. In these tests,

max the maximum consuming time is 3.437 s and the minimum L 0.7 consuming time is 0.103 s. 0 250 500 750 1000 1250 Time Sequence 9 Conclusion 0.9 A motion planning framework for variable topology truss 0.6 robots is presented in this paper, including reconfiguration (radian) and locomotion. The configuration space of a node is studied min

θ 0.3 considering the physical size of the robot components and 0 250 500 750 1000 1250 singularity avoidance constraints. Geometry reconfiguration Time Sequence planning involves the motions of multiple nodes which are 0.9 strongly coupled. An efficient algorithm to compute the obstacle regions and free space of a group is developed so that RRT can be applied to solve the geometry µ 0.5 reconfiguration planning problem easily with all hardware

0.1 constraints considered. A fast algorithm to compute all 0 250 500 750 1000 1250 enclosed subspaces in the free space of a node is presented so Time Sequence that we can verify whether topology reconfiguration actions are needed. A sample generation method is introduced Figure 34. The minimum length (Lmin) and the maximum to generate samples that can efficiently provide valid L length ( max) of all moving edge modules, the minimum angle topology reconfiguration actions over a wide space. With between every pair of edge modules (θmin), and the motion manipulability (µ) are measured throughout the locomotion our topology reconfiguration planning algorithm based on process in Figure 33. Dijkstra’s algorithm, a sequence of topology reconfiguration actions can then be computed with geometry reconfiguration planning for a group of nodes, and the motion tasks requiring are not considered in this approach and it takes 166 s to topology reconfiguration can then be solved efficiently. compute the whole process. The octahedron configuration is A non-impact rolling locomotion algorithm is presented also tested with our framework and the comparison is shown to move an arbitrary VTT easily by a simple command Liu et al. 21 with our efficient geometry reconfiguration planner. Our Kettner L (1999) Using generic programming for designing a data locomotion algorithm outperforms other approaches in terms structure for polyhedral surfaces. Computational Geometry of efficiency and robustness. These works can also be applied 13(1): 65–90. DOI:https://doi.org/10.1016/S0925-7721(99) to other truss robots. 00007-3. URL https://www.sciencedirect.com/ science/article/pii/S0925772199000073. Komendera E and Correll N (2015) Precise assembly of 3d truss References structures using mle-based error prediction and correction. Abrahantes M, Nelson L and Doorn P (2010) Modeling and The International Journal of Robotics Research 34(13): 1622– gait design of a 6-tetrahedron walker robot. In: 2010 42nd 1644. DOI:10.1177/0278364915596588. Southeastern Symposium on System Theory (SSST). pp. 248– Liu C, Whitzer M and Yim M (2019a) A distributed reconfiguration 252. DOI:10.1109/SSST.2010.5442831. planning algorithm for modular robots. IEEE Robotics and Agrawal SK, Kissner L and Yim M (2001) Joint solutions of many Automation Letters 4(4): 4231–4238. DOI:10.1109/LRA.2019. degrees-of-freedom systems using dextrous workspaces. In: 2930432. Proceedings 2001 ICRA. IEEE International Conference on Liu C and Yim M (2019) Reconfiguration motion planning for Robotics and Automation (Cat. No.01CH37164), volume 3. pp. variable topology truss. In: 2019 IEEE/RSJ International 2480–2485 vol.3. DOI:10.1109/ROBOT.2001.932995. Conference on Intelligent Robots and Systems (IROS). pp. Barber CB, Dobkin DP and Huhdanpaa H (1996) The quickhull 1941–1948. DOI:10.1109/IROS40897.2019.8967640. algorithm for convex hulls. ACM Trans. Math. Softw. 22(4): Liu C and Yim M (2021) A quadratic programming approach 469–483. DOI:10.1145/235815.235821. to manipulation in real-time using modular robots. The Butler Z, Kotay K, Rus D and Tomita K (2004) Generic International Journal of Robotic Computing 3(1): 121–145. decentralized control for lattice-based self-reconfigurable DOI:10.35708/RC1870-126268. robots. The International Journal of Robotics Research 23(9): Liu C, Yu S and Yim M (2019b) Shape morphing for variable 919–937. DOI:10.1177/0278364904044409. topology truss. In: 2019 16th International Conference on Casal A and Yim M (1999) Self-reconfiguration planning for a Ubiquitous Robots (UR). Jeju, Korea. class of modular robots. In: McKee GT and Schenker PS Liu C, Yu S and Yim M (2020) A fast configuration space algorithm (eds.) Sensor Fusion and Decentralized Control in Robotic for variable topology truss modular robots. In: 2020 IEEE Systems II, volume 3839. International Society for Optics and International Conference on Robotics and Automation (ICRA). Photonics, SPIE, pp. 246 – 257. DOI:10.1117/12.360345. pp. 8260–8266. DOI:10.1109/ICRA40945.2020.9196880. Collins F and Yim M (2016) Design of a spherical robot arm with Liu C, Yu S and Yim M (2020) Motion planning for variable the spiral zipper prismatic joint. In: 2016 IEEE International topology truss modular robot. In: Proceedings of Robotics: Conference on Robotics and Automation (ICRA). pp. 2137– Science and Systems. Corvalis, Oregon, USA. DOI:10.15607/ 2143. DOI:10.1109/ICRA.2016.7487363. RSS.2020.XVI.052. Fromherz M, Hogg T, Shang Y and Jackson W (2001) Modular Lyder A, Garcia RFM and Stoy K (2008) Mechanical design of robot control and continuous constraint satisfaction. In: Odin, an extendable heterogeneous deformable modular robot. Proceedings of IJCAI Workshop on Modelling and Solving In: 2008 IEEE/RSJ International Conference on Intelligent Problems with Contraints. Seatle, WA, pp. 47–56. Robots and Systems. pp. 883–888. DOI:10.1109/IROS.2008. Gilpin K, Kotay K, Rus D and Vasilescu I (2008) Miche: Modular 4650888. shape formation by self-disassembly. The International Miura K (1984) Design and operation of a deployable truss Journal of Robotics Research 27(3-4): 345–372. DOI:10.1177/ structure. In: NASA. Goddard Space Flight Center The 18th 0278364907085557. Aerospace Mech. Symp. Greenbelt, Maryland, pp. 49–63. DOI: Graham R (1972) An efficient algorith for determining the convex 19840017014. hull of a finite planar set. Information Processing Letters Murata S, Yoshida E, Kamimura A, Kurokawa H, Tomita K 1(4): 132 – 133. DOI:https://doi.org/10.1016/0020-0190(72) and Kokaji S (2002) M-TRAN: Self-reconfigurable modular 90045-2. robotic system. IEEE/ASME Transactions on Mechatronics Hamlin GJ and Sanderson AC (1997) TETROBOT: A modular 7(4): 431–441. DOI:10.1109/TMECH.2002.806220. approach to parallel robotics. IEEE Robotics Automation Park S, Bae J, Lee S, Yim M, Kim J and Seo T (2020) Polygon- Magazine 4(1): 42–50. DOI:10.1109/100.580984. based random tree search planning for variable geometry truss Hou F and Shen WM (2014) Graph-based optimal reconfiguration robot. IEEE Robotics and Automation Letters 5(2): 813–819. planning for self-reconfigurable robots. Robotics and DOI:10.1109/LRA.2020.2965871. Autonomous Systems 62(7): 1047 – 1059. DOI:https://doi.org/ Park S, Park E, Yim M, Kim J and Seo TW (2019) Optimization- 10.1016/j.robot.2013.06.014. based nonimpact rolling locomotion of a variable geometry Jarvis R (1973) On the identification of the convex hull of a finite truss. IEEE Robotics and Automation Letters 4(2): 747–752. set of points in the plane. Information Processing Letters 2(1): DOI:10.1109/LRA.2019.2892596. 18 – 21. DOI:https://doi.org/10.1016/0020-0190(73)90020-3. Salemi B, Moll M and Shen W (2006) SUPERBOT: A deployable, Jeong S, Kim B, Park S, Park E, Spinos A, Carroll D, Tsabedze T, multi-functional, and modular self-reconfigurable robotic Weng Y, Seo T, Yim M, Park FC and Kim J (2018) Variable system. In: 2006 IEEE/RSJ International Conference on topology truss: Hardware overview, reconfiguration planning Intelligent Robots and Systems. pp. 3636–3641. DOI:10.1109/ and locomotion. In: 2018 15th International Conference on IROS.2006.281719. Ubiquitous Robots (UR). pp. 610–615. DOI:10.1109/URAI. Spinos A, Carroll D, Kientz T and Yim M (2017) Variable topology 2018.8441880. truss: Design and analysis. In: 2017 IEEE/RSJ International 22 Journal Title XX(X)

Conference on Intelligent Robots and Systems (IROS). pp. 2717–2722. DOI:10.1109/IROS.2017.8206098. Sucan IA, Moll M and Kavraki LE (2012) The open motion planning library. IEEE Robotics Automation Magazine 19(4): 72–82. DOI:10.1109/MRA.2012.2205651. Suh JW, Homans SB and Yim M (2002) Telecubes: Mechanical design of a module for self-reconfigurable robotics. In: Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292), volume 4. pp. 4095– 4101 vol.4. DOI:10.1109/ROBOT.2002.1014385. Usevitch N, Hammond Z, Follmer S and Schwager M (2017) Linear actuator robots: Differential kinematics, controllability, and algorithms for locomotion and shape morphing. In: 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). pp. 5361–5367. DOI:10.1109/IROS.2017. 8206431. Usevitch NS, Hammond ZM and Schwager M (2020) Locomotion of linear actuator robots through kinematic planning and nonlinear optimization. IEEE Transactions on Robotics : 1– 18DOI:10.1109/TRO.2020.2995067. Woo Ho Lee and Sanderson AC (2002) Dynamic rolling locomotion and control of modular robots. IEEE Transactions on Robotics and Automation 18(1): 32–41. DOI:10.1109/70.988972. Yim M, Duff DG and Roufas KD (2000) PolyBot: A modular reconfigurable robot. In: Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065), volume 1. pp. 514–520 vol.1. DOI:10.1109/ROBOT.2000. 844106. Yim M, Shen W, Salemi B, Rus D, Moll M, Lipson H, Klavins E and Chirikjian GS (2007) Modular self-reconfigurable robot systems [grand challenges of robotics]. IEEE Robotics Automation Magazine 14(1): 43–52. DOI:10.1109/MRA.2007. 339623. Yim M, White P, Park M and Sastra J (2009) Modular self- reconfigurable robots. In: Meyers RA (ed.) Encyclopedia of Complexity and Systems Science. New York, NY: Springer New York. ISBN 978-0-387-30440-3, pp. 5618–5631. DOI: 10.1007/978-0-387-30440-3 334.