PuzzleFlex: kinematic motion of chains with loose joints

Samuel Lensgraf, Karim Itani, Yinan Zhang, Zezhou Sun, Yijia Wu,Alberto Quattrini Li, Bo Zhu, Emily Whiting, Weifu Wang, Devin Balkcom

Abstract— This paper presents a method of computingfree motions of a planar assembly of rigid bodies connectedby loose joints. Joints are modeled using local distanceconstraints, which are then linearized with respect to con-figuration space velocities, yielding a linear programmingformulation that allows analysis of systems with thousandsof rigid bodies. Potential applications include analysis ofcollections of modular robots, structural stability pertur-bation analysis, tolerance analysis for mechanical systems,and formation control of mobile robots.

I. INTRODUCTION

Like a human skeleton, structures assembled by orout of robots may be composed of rigid bodies looselyconnected at the joints. A many-jointed robot arm flexeslike the backbone of a snake; a wooden jigsaw puzzlemay flex slightly as one edge is pulled, particularlybefore assembly is complete. Joints may be real orvirtual: enforced by the physics of collision, or by robotcontrol laws that prevent the breaking of formation.

This paper studies a model of the kinematics of col-lections of rigid bodies that are flexible in the aggregate.It presents a simple, fast, linearized method to quicklyestimate potential motions of the system that maximizedeviation from the initial configuration in a considereddirection. Figure 1 shows an example of a planar puzzleflexing in such a way that the upper right block movesmaximally in the positive x direction. Because of thelinearization, there is some violation of the constraints;the paper presents time-stepping and other methods toverify the estimate while respecting constraints.

Flexibility analysis may enable wise design decisionsabout robot systems or about structures that robotsbuild. Flexibility may be good, allowing compliancewith external forces, or bad, reducing the sturdiness andpredictability of the system. What joint tolerances enableassembly, while providing either enough flexibility forLego-like bricks or modular robots to comply to an ex-ternal structure, or enough rigidity for the robots to resistexternal loads? What arrangements of bodies provide thedesired level of flexibility? How much motion, and in

This project was partially supported by NSF Grant 1813043, as wellas by the Dubai Future Foundation.

Fig. 1: A system of eight rigid blocks (left), with theupper-right block flexing to the right.

which direction, can a system of flocking robots achievewhile maintaining constraints such as mutual visibility?

Figure 2 shows a system of particular interest tothe authors: a chair created from Lego-like blocks heldtogether by a puzzle-like arrangement, rather than byfriction or glue. We imagine constructing such structuresautomatically with robots, or from systems of modularrobots. The chair flexes slightly, but remains as a singlecomponent as long as the final block assembled isheld in place. Fast analysis of flexibility will allowspecific design decisions: reinforcing the chair by addingother blocks, or increasing tolerance at joints to alloweasier manufacturing and assembly, while maintainingacceptable rigidity. For simplicity, this paper focuses onplanar systems, but this limitation is not fundamental.

We developed a Julia library to build the linearconstraint matrix based on geometric descriptions of partgeometry1, and used Gurobi [1] to solve linear programs.Though we compute and present distance function gra-dients in the paper for reference, we used automaticdifferentiation in the implementation for simplicity [2].Figure 9 shows a structure with 1703 blocks, with ydisplacement of the upper right block maximized usingthis implementation.

II. RELATED WORK

Linearizing motion around an initial configuration al-lows for the study of systems of blocks with many thou-sand degrees of freedom; our approach draws inspirationfrom early work on manipulability ellipsoids [4]–[7],in which directions of motion of the end effector of a

1Software available from rlab.cs.dartmouth.edu

arX

iv:1

906.

0870

8v1

[cs

.RO

] 2

0 Ju

n 20

19

Fig. 2: Interlocking puzzle blocks, from [3].

robot arm are analyzed at a particular configuration byexamining eigenvectors of the Jacobian matrix. Work byBerenson also provides analysis and approximations ofJacobians for truly flexible cloth or string [8]. Lineargrasp analysis techniques also serve as inspiration. Inthe 19th century, Reuleaux [9] derived a geometricmethod to find the free motion of an object in contactwith frictionless fingers. Mishra, Schwartz, and Sharir’sseminal work on the minimum number and sufficientplacement of fingers to immobilize an object [10] ana-lyzes polyhedral constraints in twist and wrench spaces.

In contrast to manipulability and grasping problems,the blocks which we consider are only loosely con-nected. Caging grasps [11]–[17] study how robot handsmay loosely capture an object; the present paper studiesmotion of structures in which either pairs of blocksor combinations of many blocks may cage each other.Direct construction of configuration spaces of pairs ofblocks has a long history; Sacks et al. [18] provides arecent approach, and gives a much higher-fidelity repre-sentation of the free motions of small numbers of blocksthan our edge/point distance function model. Ecksteinet al. [19] analyze how forgiving a connector designis using an explicit approximation of the configurationspace of the joint.

The Carpenter’s Rule Theorem states that any openpolygonal chain (a planar revolute robot arm) can bereconfigured arbitrarily without self-intersection [20];the proof uses expansive motions that cause points andedges to separate from one another. The motions inthe present paper allow points and edges to approachone another, while balancing the rates so as to optimizenet motion in some direction. The distance constraintsare similar to those used in Linear ComplementarityProblem (LCP) formulations of dynamics [21], [22],which have been used both for rigid body simulationand design for manipulation [23].

Tolerance analysis of mechanical assemblies is uti-lized in mechanical engineering to determine how fre-quently small manufacturing errors in the componentparts of an assembly will result in unacceptable devia-

tions in the final assembly [24]. The Direct LinearizationMethod [25] linearizes the homogeneous transformationmatrices describing the kinematics of an assembly, andapplies statistical techniques to determine what percent-age of assemblies are able to be assembled.

Methods for building modular interlocking structureshave been studied by Zhang et al. [3], [26] and byWerfel et al. [27]; however, the structures are assumedto be static after construction with idealized perfectconnectors between blocks. Techniques for robot swarmcontrol typically must handle thousands of simple robotscollectively performing some tasks, e.g., object trans-ports [28], shape generation [29], self-assembly [30],[31], and network connectivity [32]; perhaps the closestwork in spirit to the present is [33], which controlsswarms of robots by allowing robots to bounce off offrictionless walls.

III. LINEARIZED DISTANCE FUNCTIONS

Let the configuration of the chain be given by q ∈ Q.Define two types of points of interest: vertices of thepolygons describing each body in the chain o(q) andcollision points p(q). Define a vector of signed distancefunctions that represents the distance of each collisionpoint from its neighboring edges: d(o,p). Componentsof the vector d will be notated by di,j , where i is theindex of the edge and j is the index of the point. Toenforce that there are no collisions, d(q) ≥ 0.

To analyze legal motion and legal nearby configura-tions of the chain, we may consider the configurationto be a function of time: q(t). Let q ∈ TQ be aconfiguration-space direction indicating possible motionof the system. The instantaneous rate of change of thedistance function is

d(q, q) = Jd(q)q, (1)

where Jd is the Jacobian of the distance function. For asmall enough time step ∆t, an Euler step approximatesthe change in distances:

∆d(t) ≈ ∆td(q, q). (2)

Let d0 = d(o0,p0) be the distances computed atthe initial configuration of the chain. We would liketo choose motions such that the change in distancesfrom each collision point to each edge does not causea collision: ∆d(t) ≤ d0. Combining with Equations 1and 2,

Jd(q)q + d0 ≥ 0. (3)

The scalar ∆t has been dropped, since we may equiv-alently linearly scale q and scale time units such that∆t = 1. With this time scaling, the change of q over a

2

0.0 0.5 1.0 1.5 2.0

0.00

0.25

0.50

0.75

1.00

1.25

1.50

1.75

2.00

1

2

34

Fig. 3: 1R planar arm, and estimates of end-effectorcollisions.

time step is approximated by ∆q = q. Thus, Equation 3bounds the change in configuration to a polyhedron.

IV. A SIMPLE EXAMPLE

Consider a 1R robot arm with base at the origin, and asingle link of length 2, shown in Figure 3. The configu-ration q is the angle θ; let the initial configuration be θ =π/4. Constrain the endpoint of the arm to lie in a squareregion with vertices o = ((1, 1), (2, 1), (2, 2), (1, 2)).The end effector coordinates are

p(q) = (2 cos θ, 2 sin θ). (4)

There are four distance functions:

d1 = py − o1y = 2 sin θ − 1 (5)d2 = −(px − o2x) = −2 cos θ + 2 (6)d3 = −(py − o3y = −2 sin θ + 2 (7)d4 = px − o4x = 2 cos θ − 1, (8)

corresponding to distances from the bottom, right, top,and left walls. Computing the partial derivatives withrespect to θ,

Jd(q)q + d0 =

2 cos θ2 sin θ−2 cos θ−2 sin θ

q + d0 ≥ 0 (9)

√

2√2

−√

2

−√

2

q +

√

2− 1

2−√

2

2−√

2√2− 1

≥ 0. (10)

p1

p2

o1

o2

e1

e2 e3 e4

(a) The edge-vertex distanceconstraints limit the validmotions in a small convexregion.

(b) The distance constraintsgenerated for a pair of rigidbodies. Blue line segmentsare vectors from green ver-tices to red edges.

Fig. 4: Local distance functions model the free space.

Candidate values for q, or equivalently, ∆q, are then−(√

2− 1)/√

2

−(2−√

2)/√

2

−(√

2− 2)/√

2

−(1−√

2)/√

2

≈−.29−.41.41.29

. (11)

The value ∆θ = −.29 corresponds to collision withthe bottom wall, and the value ∆θ = .29 correspondsto collision with the left wall; these would be thefirst collisions to occur. These values are of courseapproximate, due to the linearization of J around theinitial configuration.

V. FLEXIBILITY ANALYSIS USING LINEARPROGRAMMING

We apply the concepts developed above to approxi-mating extreme configurations of very large 2D systemsof loosely connected rigid bodies by solving the linearprogram

maxq

cT q

subject to J(q)q + d0 ≥ 0,(12)

where c is a vector of weights. The choice of c allowsus to tune the direction we wish to displace elements ofthe chain.

We use signed distance functions between vertices onone body and edges on another to simply model thepermissible local motions of the bodies. For each pairof bodies in the structure, we choose one body to providethe edges, and one body to provide vertices, as shown inFigure 4a. Since the linearized analysis is only valid forlocal motions of the bodies, only edges and points thatare initially near one another are potential sources ofcollision; we choose a small positive value ε and selectedge/vertex pairs that are initially closer than this value.

Let the configurations of the current pair of bodiesunder consideration be q1 = (x1, y1, θ1) and q2 =(x2, y2, θ2). For each object pair, we expect there to be

3

many distance functions, representing the distances ofvertices from edges over some region of near-contactbetween the bodies. For simplicity, consider a distancefunction dij such that object 1 provides the edge, andobject 2 provides the vertex. Let n be the outwards-pointing normal from the edge and o be the origin. Then

dij(q1,q2) = n(q1) · (p(q2)− o(q1)). (13)

Let the length of the edge be `, the first and secondendpoints of the edge be e0 and e1, the distance from theorigin of object 1 to the endpoints of the edges be re0and re1 and the angle from the x axis in the local frameof object 1 to the edge endpoints be αe0 and αe1 . Let thedistance from p to the origin in the local frame of object2 be rp and the angle from the x axis to p be αp. Tomake the equations more readable, we define the helpervariables se1 = sin (θ1 + αe1), ce1 = cos (θ1 + αe1),se0 = sin (θ0 + αe0), ce0 = cos θ1 + αe0 , a = 1

` . Then

p(q2) =

(x2 + rpcpy2 + rpsp

)(14)

n(q1) = a

(re1se1 − re0se0−re1ce1 + re0ce0

)(15)

o(q1) =

(x1 + re0ce0y1 + re0se0

). (16)

The non-zero entries of each row of the Jacobian arecomputed using the gradients of the distance function,substituting the appropriate blocks for blocks 1 and 2:

∂x1dij = a (−re0se0 + re1se1)

∂y1dij = a (ce0re0 − ce1re1)

∂θ1dij = a(ce0re0 (ce0re0 − ce1re1)

− re0se0 (−re0se0 + re1se1)

+ (−ce0re0 + ce1re1) (−cprp + ce0re0 + x1 − x2)

+ (−re0se0 + re1se1) (−rpsp + re0se0 + y1 − y2))

∂x2dij = −a (−re0se0 + re1se1)

∂y2dij = −a (ce0re0 − ce1re1)

∂θ2dij = a(−cprp (ce0re0 − ce1re1)

+ rpsp (−re0se0 + re1se1))

(17)

A. Modeling convex corners

The distance-function approximation of the local con-figuration space is particularly bad for some objectgeometries. In Figure 4a, point p1 is closest to pointo1, a convex corner. Two distance functions are created,one for each of the extension into lines of the edgese1 and e2. Maintaining these constraints unnecessarilyrestricts p1; p1 will remain in the polygonal regiondefined by the extensions of e1 and e2. This problem

seems fundamental. Rows of the Jacobian express anand relationship; all constraints must be satisfied. Butin the example, it is enough that p1 be on the “correct”side of only one of the extended edges.

If only one of the nearby vertices is convex, theproblem is easily solvable. For example, in Figure 4a,points o2 and p2 may be swapped, so that we computethe distance of a point relative to a concave corner. Tomitigate the problem in the case where both cornersare convex, we may take a simple, though not entirelysatisfactory, approach. Take the normals of each edge,and average them, yielding a half-plane constraint that atleast allows p1 to cross over the extended edges. Oncean edge has been crossed, it is no longer treated as avalid constraint and dropped from the Jacobian, allowinga second time-step to more accurately model the motionof p1. Deeper exploration of this issue is a primary goalof future work; one promising avenue is formulation asa Linear Complementarity Problem (LCP), allowing orrelationships between constraints.

B. Time-stepping and re-enforcement of constraints

Solutions to the linear program in Equation 12 areextreme vertices of the constraint polyhedron. Becauseof the linearization around the initial configuration, theconstraints may be violated when the resulting solutionis used to compute a new configuration. The linearizeddistance functions are Taylor series approximations,truncated after the first term. A common approach fordealing with truncation error in finite difference methodsis to find the net change over several time steps.

Although there are sophisticated ways to computean optimal time step for finite difference methods, forthis problem, the cost of computing the linear programsolution far outweighs the cost of Euler-step integrationand forward kinematics distance computation. We takea simple approach, and do a linear or binary search fora time step, multiplying the displacement vector ∆q byan increasingly large scalar until the maximum distanceconstraint violation exceeds a user-defined threshold.After a time step is found and applied, a new linearprogram may be formulated and solved around the newconfiguration q.

Usefully, the new linear program re-enforces the con-straints, potentially taking a backward step with ∆q.This means that error does not accumulate across timesteps, and also suggests an even more computationallyefficient, though numerically riskier, approach: computea single-step solution that violates the constraints, andthen solve just one more linear program. In future work,we expect to analytically and empirically explore cir-cumstances under which this faster method is sufficient.

4

Fig. 5: Example of adding a cross beam into a structureat a point of maximum flex.

Fig. 6: Crushing a soda can with tight and loose joints.Red polygons denote the initial configuration.

VI. EXAMPLE PROBLEMS

In this section, we present some informal examples –preliminary work that suggests a breadth of interestingapplications.

A. Structure and block design problems

The linear optimization approach may be fast enoughfor rapid consideration of different potential designsfor a structure, including the number and locations ofblocks, and on the the geometry of individual blocks,including the tightness of the joints.

As an example, we consider how to add blocks tobrace a structure and limit maximum flex. Figure 5shows an example of adding such a beam to a structure;c was chosen to maximize radial flex of each blockoutwards from the center. We find the pair of mutuallyvisible vertices which has changed the most in thepredicted configuration of maximum flex, an O(n2)operation for n vertices. In this example, this approachsuggested adding a vertical cross-beam of blocks, whichwe did by hand. In a completely automated algorithm,structural limitations of the blocks would need to betaken into account when selecting a cross beam.

The linear programming approach can also be used toexplore joint geometry. Looser joints simplify assembly;if the joints in Figure 2 are too tight, the chair cannot beassembled due to limits on the precision of assembly andfabrication. However, if joints are too loose, the structure

Fig. 7: A linear programming solution for a flock of1024 robots which must maintain sensor contact squeez-ing together to fit through a doorway or hallway.

will flex unacceptably, particularly if there is wear on theconnectors over time. Figure 6 (right) shows a planarexample of the soda can with loosened joints, with flexcomputed using linear programming.

One simple strategy to explore joint tolerance is toparameterize the tightness of a joint with a single valueand binary search for maximum tolerance. To simulatesuch a process, we utilize the Clipper library [34] tosimulate loosening joints by insetting the boundary ofthe rigid bodies. A more general approach might chooseseveral parameters to describe joint geometry, and searchover this parameter space. A key question is how tochoose c to test flex; Subsection VI-C shows how todiscover such a direction in the extreme case that jointsseparate and break the structure.

B. Flock formations

Figure 7 shows a flock of 1024 robots; the magnifiedinset shows the geometry. Gray square robots are forbid-den from physical collision, and the yellow cone showsa requirement that each robot’s camera must maintainview of a marker (red dot) on the robot in front of it.We can drive the flock into interesting configurations byselecting an objective function. Figure 7 shows an exam-ple: driving the diffuse flock into a tighter configuration(perhaps so that the robots can pass through a doorway)by finding a displacement that moves all of the robotstoward the x value of the leader robot.

We add field-of-view constraints for each robot exceptthe leader, and collision constraints that require thatvertices of each robot do not cross the half planesdescribed by the edges of its five nearest robots. Weadded a constraining square around the leader at the tipof the tree so that the constraint polyhedron is bounded.Large rotations are poorly approximated by the linearmethod, so we place an arbitrary limit on the rotationdisplacement of each robot in a time step, using auxiliary

5

Fig. 8: A small time step in a direction of separation.

linear constraints. After each configuration update, were-select distance constraints between swarm neighbors.

C. Unbounded separation and (dis-)assembly planning

The classic assembly problem [35], [36] is to discovermotions that separate or assemble a collection of rigidbodies. For simple versions of this problem, we mightlike to discover a velocity direction q for which thelinear constraints we formulated are unbounded. Withsome minor modifications, our approach is able to dis-cover such a direction, thus testing whether a structure isinterlocked under constant-velocity motion, which mayinclude rotation.

Linear program solvers are capable of detectingwhether the feasible polyhedron is unbounded in thedirection of a given cost vector; in contrast, we wouldlike to discover such a cost vector automatically. Ourapproach is based on the observation that for almostevery non-zero vector, the linear sum of the elementsis either positive or negative, but not zero. We maycompute the sum of the x and y elements of q byadding a row of the form (1, 1, 0, 1, 1, 0, . . . ) to J . Wemay constrain that sum to be very large, by adding anadditional large element k to d0. Choose the objectivefunction c arbitrarily. We must also upper bound themotion so that the solution is not unbounded; we add arow (−1,−1, 0, . . .) and an element −2k to d0.

If a solution is found to this linear program, thenthe resulting q removes at least one block far enoughfrom the assembly that it is unconstrained, allowingunbounded motion. If not, then we may look for negativemotions by changing the signs on the last two elementsof d0. If both of these linear programs are infeasible,then the only separating motions must be such that thesum of the x and y velocity elements is exactly 0. Weneglect this case in our implementation, but perhapsit might be handled if desired by rotating the entirestructure slightly, changing the relationship between xand y velocities along the separation direction.

Our study of this approach is preliminary; issues ofnumerical stability may arise that we have not discov-ered. We have explored a few examples; Figure 8 shows

Rigid Bodies Jacobian Size Iterations Runtime (seconds)36 666 x 105 5 0.35850 907 x 147 8 0.36390 2220 x 267 7 1.621153 3147 x 456 8 1.866223 5273 x 666 5 2.987332 6309 x 993 5 2.761392 10932 x 1173 8 10.649396 7326 x 1185 4 2.002688 14615 x 2061 4 5.951

1703 52655 x 5106 14 233.3412497 66363 x 7488 7 166.582

TABLE I: Performance results for several structures,using Gurobi with sparse matrices.

Fig. 9: Structure composed of 1703 rigid bodies flexingupwards. Red polygons denote the initial configuration.

an example of a direction of separation found by thismethod. As will be discussed in the limitations sectionbelow, this approach gives little control over whichdirection of separation is selected. Future work mayexplore non-linear objective functions or direct analysisof the constraint polyhedron, perhaps by computingextreme edges, the double description problem [37].

VII. EVALUATION AND COMPARISONS

The size of the linear program depends on the numberof blocks, the complexity of their shape, and the waysin which they are connected; the number of time-stepiterations depends on the flexibility of the structure withrespect to angular motions in configuration space. In thissection, we explore the time costs, evaluated in terms ofthe size of the linear programs, number of steps, and thepractical run-time on a current desktop system.

For n blocks with one block held fixed, the Jacobianhas 3(n − 1) columns and c(n − 1) rows, if c isthe average number of distance constraints generatedfor each block. However, the matrix is quite sparse,which may reduce memory and computational costs ofsolution; there are only six non-zero entries per row,yielding O(n) non-zero entries in the matrix. We omitformal O() asymptotic run-time analysis of the solution,since the solution techniques are standard for linearprogramming.

6

1

2

34

5

Fig. 10: Motions of a rectangle constrained to a box.

In Table I we show the result of tests on severalsystems of rigid bodies of varying size. For each struc-ture, we report the amount of time and number ofiterations required for our time stepping procedure toconverge. The run time of our approach is dominated bythe solution of the constraint Jacobian linear program.In our experiments, we found that certain instanceswere especially hard for the linear program solver. Forinstance, the 392 rigid body structure takes five times aslong the 396 body structure to solve and two times aslong as the 688 body structure. The 392 body structureis a very dense structure, making the placement of eachrigid body dependent on a larger number of other rigidbodies than in less dense structures.

VIII. LIMITATIONS AND FUTURE WORK

We presented a simple linear-constraint method forcomputing the motion of a loosely-connected chain ofrigid bodies. Like robot kinematics formulations, theapproach is geometric, and does not model dynamicsand contact. This is both a strength and a weakness;dynamics simulators may provide realistic motions, butthe linear constraints describe a space of possible motionof the system, allowing fast and interesting optimiza-tions. The linear constraint method may also be moreuseful for a worst-case analysis; just because a simulatorprovides a trajectory does not mean that trajectory willoccur in the real world.

We have begun to conduct a comparison to state-of-the-art dynamics simulation approaches, using externalforces applied to the rigid bodies as a loose parallel tothe objective function c used for the linear program.We have simulated a simple beam of 50 blocks, andsolutions are similar. However, it is not immediatelyclear how to choose these external forces for dynamicsimulation of more complex structures, since we ex-pect that for some systems, counter-intuitive motions ofsome blocks will lead to maximized flex. Computationalcosts are difficult to compare; for a dynamic simulator,reaching a flexed configuration requires many time stepsthrough impact events, but for the linear program, many

possible alternative motions are analyzed to maximizethe objective. Future work will attempt to make moredirect comparisons.

The linear-constraint method assumes that the con-figuration space is tight enough that linearizationof the change in distance functions with respect toconfiguration-space motion is not too inaccurate. Forstructures that are nearly rigid, as we might like to buildin assembly problems, this assumption is reasonable,and the one-step solution to the linear program givesgood results. However, for more flexible systems, thecomputed motions violate the distance constraints. Re-peated enforcement of the constraints by time-steppingand re-solving the linear program gives results that seemempirically reasonable, but there is much to be doneto put this approach on firmer mathematical footing,perhaps by analyzing Taylor series approximations ofthe change in distance functions [38].

The use of a linear objective function is also limiting.Figure 10 shows an example of a rectangular robot ina square room. Attempting to maximize θ rotates therobot from 1, to 2, to 3, to 4, and to 5 in successive timesteps, but because θ is unbounded, time-stepping will notconverge. Other interesting problems are also an imper-fect match for linear objective functions. While we cananalyze separability of objects using the approach out-lined in Section VI-C, there is little control over whichseparating motion is selected by the linear program. Wemight like to separate objects in an assembly one attime (if we have only one robot arm), or simultaneously,for speed; it is unclear how these preferences might beencoded with linear objective functions.

The use of the union of edge-vertex distance con-straints to approximate the local configuration spacealso needs further study; as pointed out in Section V-A, convex corners of objects pose a particular problemwhen used as edges for the distance function. Extensionto 3D, an obvious next step for the work, seems mostlystraight-forward, but we expect expressing the geometryof convex vertices, saddles, and ridges using a union oflinear constraints to be more problematic than in the 2Dcase.

REFERENCES

[1] L. Gurobi Optimization, Gurobi optimizer referencemanual, 2018. [Online]. Available: http://www.gurobi.com.

[2] J. Revels, M. Lubin, and T. Papamarkou,“Forward-mode automatic differentiation in julia,”ArXiv:1607.07892 [cs.MS], 2016.

[3] Y. Zhang and D. Balkcom, “Interlocking block assem-bly,” in Proc. WAFR, Dec. 2018.

7

[4] P. Chiacchio, S. Chiaverini, L. Sciavicco, and B. Sicil-iano, “Global task space manipulability ellipsoids formultiple-arm systems,” IEEE Trans. Robot. Autom., vol.7, no. 5, pp. 678–685, 1991.

[5] F. C. Park and J. W. Kim, “Manipulability and singu-larity analysis of multiple robot systems: A geometricapproach,” in Proc. ICRA, 1998, pp. 1032–1037.

[6] S. Kim, “Adjustable manipulability of closed-chainmechanisms through joint freezing and joint unactu-ation,” in Proc. ICRA, 1998, pp. 2627–2632.

[7] A. Bicchi and D. Prattichizzo, “Manipulability of coop-erating robots with unactuated joints and closed-chainmechanisms,” IEEE Trans. Robot. Autom., vol. 16, no.4, pp. 336–345, Aug. 2006.

[8] D. Berenson, “Manipulation of deformable objectswithout modeling and simulating deformation,” in Proc.IROS, 2013, pp. 4525–4532.

[9] F. Reuleaux, The kinematics of machinery. 1876.[10] B. Mishra, J. T. Schwartz, and M. Sharir, “On the

existence and synthesis of multifinger positive grips,”Algorithmica, vol. 2, pp. 541–558, 1987.

[11] A. Rodriguez, M. T. Mason, and S. Ferry, “From cagingto grasping,” Int. J. Robot. Res., vol. 31, no. 7, pp. 886–900, 2012.

[12] S. Makita and Y. Maeda, “3d multifingered caging:Basic formulation and planning,” in Proc. IROS, 2008,pp. 2697–2702.

[13] M. Vahedi and A. F. van der Stappen, “Caging polygonswith two and three fingers,” Int. J. Robot. Res., vol. 27,no. 11-12, pp. 1308–1324, 2008.

[14] J. Erickson, S. Thite, F. Rothganger, and J. Ponce,“Capturing a convex object with three discs,” in Proc.ICRA, vol. 2, 2003, pp. 2242–2247.

[15] E. Rimon and A. Blake, “Caging 2d bodies by 1-parameter two-fingered gripping systems,” in Proc.ICRA, 1996, pp. 1458–1464.

[16] T. F. Allen, J. W. Burdick, and E. Rimon, “Two-finger caging of polygonal objects using contact spacesearch,” IEEE Trans. Robot., vol. 31, no. 5, pp. 1164–1179, 2015.

[17] S. Makita and W. Wan, “A survey of robotic cagingand its applications,” Advanced Robotics, vol. 31, no.19-20, pp. 1071–1085, 2017.

[18] E. Sacks, N. Butt, and V. Milenkovic, “Robust freespace construction for a polyhedron with planar mo-tion,” Computer-Aided Design, vol. 90, pp. 18–26,2017.

[19] N. Eckenstein and M. Yim, “Modular robot connectorarea of acceptance from configuration space obstacles,”in Proc. IROS, 2017, pp. 3550–3555.

[20] R. Connelly, E. D. Demaine, and G. Rote, “Straighten-ing polygonal arcs and convexifying polygonal cycles,”Discrete and Computational Geometry, vol. 30, no. 2,pp. 205–239, Sep. 2003.

[21] D. Stewart and J. Trinkle, “Dynamics, friction, andcomplementarity problems,” in Complementarity andVariational Problems, M. Ferris and J. Pang, Eds.,SIAM, 1997, pp. 425–439.

[22] J. Trinkle, J. Tzitzouris, and J. Pang, “Dynamic multi-rigid-body systems with concurrent distributed con-tacts: Theory and examples,” Philosophical Transac-

tions: Mathematical, Physical, and Engineering Sci-ences, A, vol. 359, no. 1789, pp. 2575–2593, Dec.2001.

[23] D. Balkcom and J. C. Trinkle, “Computing wrenchcones for planar rigid body contact tasks,” Int. J. Robot.Res., vol. 21, no. 12, pp. 1053–1066, 2002.

[24] K. W. Chase and A. R. Parki nson, “A survey ofresearch in the application of tolerance analysis tothe design of mechanical assemblies,” Research inEngineering Design, vol. 3, no. 1, pp. 23–37, Mar.1991.

[25] K. W. Chase, J. Gao, S. P. Magleby, and C. D.Sorensen, “Including geometric feature variations intolerance analysis of mechanical assemblies,” IIETransactions, vol. 28, no. 10, pp. 795–807, 1996.

[26] Y. Zhang and D. Balkcom, “Interlocking structureassembly with voxels,” in Proc. IROS, 2016.

[27] J. Werfel, Y. Bar-Yam, D. Rus, and R. Nagpal, “Dis-tributed construction by mobile robots with enhancedbuilding blocks,” in Proc. ICRA, 2006, pp. 2787–2794.

[28] J. Alonso-Mora, S. Baker, and D. Rus, “Multi-robotformation control and object transport in dynamic en-vironments via constrained optimization,” Int. J. Robot.Res., vol. 36, no. 9, pp. 1000–1021, 2017.

[29] M. A. Hsieh, V. Kumar, and L. Chaimowicz, “Decen-tralized controllers for shape generation with roboticswarms,” Robotica, vol. 26, no. 5, pp. 691–701, 2008.

[30] I. O’Hara, J. Paulos, J. Davey, N. Eckenstein, N. Doshi,T. Tosun, J. Greco, J. Seo, M. Turpin, V. Kumar, et al.,“Self-assembly of a swarm of autonomous boats intofloating structures,” in Proc. ICRA, 2014, pp. 1234–1240.

[31] M. Rubenstein, A. Cornejo, and R. Nagpal, “Pro-grammable self-assembly in a thousand-robot swarm,”Science, vol. 345, no. 6198, pp. 795–799, 2014.

[32] J. M. Esposito and T. W. Dunbar, “Maintaining wirelessconnectivity constraints for swarms in the presence ofobstacles,” in Proc. ICRA, 2006, pp. 946–951.

[33] S. Shahrokhi, A. Mahadev, and A. T. Becker, “Algo-rithms for shaping a particle swarm with a shared inputby exploiting non-slip wall contacts,” in Proc. IROS,2017, pp. 4304–4311.

[34] A. Johnson, Clipper - an open source freeware libraryfor clipping and offsetting lines and polygons. [Online].Available: http : / / angusj . com / delphi /clipper.php.

[35] D. Halperin, J. Latombe, and R. H. Wilson, “A generalframework for assembly planning: The motion spaceapproach,” Algorithmica, vol. 26, no. 3-4, pp. 577–601,2000.

[36] J. Snoeyink and J. Stolfi, “Objects that cannot be takenapart with two hands,” Discrete and ComputationalGeometry, vol. 12, pp. 367–384, 1994.

[37] K. Fukuda and A. Prodon, Double description methodrevisited, 1996.

[38] J. J. Duistermaat and J. A. C. Kolk, “Taylor expansionin several variables,” in Distributions: Theory andApplications. Birkhauser, 2010, pp. 59–63.

8