PuzzleFlex: kinematic motion of chains with loose joints

TL;DR

PuzzleFlex introduces a linear programming approach to analyze the free motions of planar rigid body assemblies with loose joints, enabling efficient analysis of large systems for robotics, stability, and control applications.

Contribution

It models loose joints as local distance constraints and linearizes them for linear programming, allowing scalable analysis of complex mechanical systems.

Findings

Efficient linear programming formulation for large assemblies.

Applicable to modular robots, stability, and formation control.

Enables analysis of systems with thousands of bodies.

Abstract

This paper presents a method of computing free motions of a planar assembly of rigid bodies connected by loose joints. Joints are modeled using local distance constraints, which are then linearized with respect to configuration space velocities, yielding a linear programming formulation that allows analysis of systems with thousands of rigid bodies. Potential applications include analysis of collections of modular robots, structural stability perturbation analysis, tolerance analysis for mechanical systems, and formation control of mobile robots.

Figures

Tables

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

Rigid Bodies	Jacobian Size	Iterations	Runtime (seconds)
36	666 x 105	5	0.358
50	907 x 147	8	0.363
90	2220 x 267	7	1.621
153	3147 x 456	8	1.866
223	5273 x 666	5	2.987
332	6309 x 993	5	2.761
392	10932 x 1173	8	10.649
396	7326 x 1185	4	2.002
688	14615 x 2061	4	5.951
1703	52655 x 5106	14	233.341
2497	66363 x 7488	7	166.582

Equations

$$\dot{\mathbf{d}}(\mathbf{q},\dot{\mathbf{q}})=J_d(\mathbf{q})\dot{\mathbf{q}},$$

$$\Delta \mathbf{d}(t)\approx \Delta t\dot{\mathbf{d}}(\mathbf{q},\dot{\mathbf{q}}).$$

$$J_d(\mathbf{q})\dot{\mathbf{q}}+{\mathbf{d}}_0\ge 0.$$

$$\mathbf{p}(\mathbf{q})=(2\cos \theta ,2\sin \theta ).$$

$$d_1$$

$$d_2$$

$$d_3$$

$$d_4$$

$$J_d(\mathbf{q})\dot{\mathbf{q}}+{\mathbf{d}}_0=\left(\begin{array}{c}2\cos \theta \\ 2\sin \theta \\ -2\cos \theta \\ -2\sin \theta \end{array}\right)\dot{\mathbf{q}}+{\mathbf{d}}_0\ge 0$$

$$\left(\begin{array}{c}\sqrt{2}\\ \sqrt{2}\\ -\sqrt{2}\\ -\sqrt{2}\end{array}\right)\dot{\mathbf{q}}+\left(\begin{array}{c}\sqrt{2}-1\\ 2-\sqrt{2}\\ 2-\sqrt{2}\\ \sqrt{2}-1\end{array}\right)\ge 0.$$

$$\underset{\dot{q}}{\max }$$

$$J(q)\dot{\mathbf{q}}+{\mathbf{d}}_0\ge 0,$$

$${\mathbf{d}}_{ij}({\mathbf{q}}_1,{\mathbf{q}}_2)=\mathbf{n}({\mathbf{q}}_1)\cdot (\mathbf{p}({\mathbf{q}}_2)-\mathbf{o}({\mathbf{q}}_1)).$$

$$\mathbf{p}({\mathbf{q}}_2)$$

$$\mathbf{n}({\mathbf{q}}_1)$$

$$\mathbf{o}({\mathbf{q}}_1)$$

$${\partial }_{x_1}{\mathbf{d}}_{ij}$$

$${\partial }_{y_1}{\mathbf{d}}_{ij}$$

$${\partial }_{{\theta }_1}{\mathbf{d}}_{ij}$$

$$-r_{e_0}{s}_{e_0}\left(-r_{e_0}{s}_{e_0}+r_{e_1}{s}_{e_1}\right)$$

$$+\left(-c_{e_0}{r}_{e_0}+c_{e_1}{r}_{e_1}\right)\left(-c_p{r}_p+c_{e_0}{r}_{e_0}+x_1-x_2\right)$$

$$+\left(-r_{e_0}{s}_{e_0}+r_{e_1}{s}_{e_1}\right)\left(-r_p{s}_p+r_{e_0}{s}_{e_0}+y_1-y_2\right))$$

$${\partial }_{x_2}{\mathbf{d}}_{ij}$$

$${\partial }_{y_2}{\mathbf{d}}_{ij}$$

$${\partial }_{{\theta }_2}{\mathbf{d}}_{ij}$$

$$+r_p{s}_p\left(-r_{e_0}{s}_{e_0}+r_{e_1}{s}_{e_1}\right))$$

Full text

©2020 IEEE. Personal use of this material is permitted. Permission from IEEE must be obtained for all other uses, in any current or future media, including reprinting/republishing this material for advertising or promotional purposes, creating new collective works, for resale or redistribution to servers or lists, or reuse of any copyrighted component of this work in other works. DOI: 10.1109/ICRA40945.2020.9196854

I Introduction

Like a human skeleton, structures assembled by or out of robots may be composed of rigid bodies loosely connected at the joints. A many-jointed robot arm flexes like the backbone of a snake; a wooden jigsaw puzzle may flex slightly as one edge is pulled, particularly before assembly is complete. Joints may be real or virtual: enforced by the physics of collision, or by robot control laws that prevent the breaking of formation.

This paper studies a model of the kinematics of collections of rigid bodies that are flexible in the aggregate. It presents a simple, fast, linearized method to quickly estimate potential motions of the system that maximize deviation from the initial configuration in a considered direction. First, a set of linear constraints is derived that approximates the shape of the local configuration space; then linear programming is used in various ways to optimize or analyze potential motions of the system. Figure 1 shows an example of a planar puzzle flexing in such a way that the upper right block moves maximally in the positive $x$ direction. Because of the linearization, there is some violation of the constraints; the paper presents time-stepping and other methods to verify the estimate while respecting constraints.

Flexibility analysis may enable wise design decisions about robot systems or about structures that robots build. Flexibility may be good, allowing compliance with external forces, or bad, reducing the sturdiness and predictability of the system. What joint tolerances enable assembly, while providing either enough flexibility for Lego-like bricks or modular robots to comply to an external structure, or enough rigidity for the robots to resist external loads? What arrangements of bodies provide the desired level of flexibility? How much motion, and in which direction, can a system of flocking robots achieve while maintaining constraints such as mutual visibility?

The work is motivated by methods for building modular interlocking structures such as those presented by Zhang et al. [1, 2] and by Werfel et al. [3]. Figure 2 shows a system of particular interest to the authors: a chair created from Lego-like blocks held together by a puzzle-like arrangement, rather than by friction or glue. We imagine constructing such structures automatically with robots, or from systems of modular robots. The chair flexes slightly, but remains as a single component as long as the final block assembled is held in place. Fast analysis of flexibility will allow specific design decisions: reinforcing the chair by adding other blocks, or increasing tolerance at joints to allow easier manufacturing and assembly, while maintaining acceptable rigidity. For simplicity, this paper focuses on planar systems.

We developed a Julia library to build the linear constraint matrix based on geometric descriptions of part geometry111Software available from rlab.cs.dartmouth.edu, and used Gurobi [4] to solve linear programs. Though we compute and present distance function gradients in the paper for reference, we used automatic differentiation in the implementation for simplicity [5]. Figure 9 shows a structure with 1703 blocks, with $y$ displacement of the upper right block maximized using this implementation.

While the present work makes use of sparse linear programming to solve for maximal motions in a direction in the configuration space of the chain, the convex polytope that approximates the shape of the local configuration space is interesting in itself. We note that global optimization techniques over the configuration space without first estimating the local constraint region, are in some ways too strong – they may find solutions that are not connected to the initial configuration space, and are thus not reachable. Ultimately, we would like to move beyond optimizaton and explore ways of measuring properties of this polytope, and use these properties to automatically design structures, in much the same way as the manipulablity elipsoid constructed from the Jacobian has long been used in robot arm design [6, 7, 8, 9].

II Related work

A slightly extended version of this paper is available as a technical report [10]. The work closest to the present in spirit is on compaction of planar polygons [11]; the present work differs in assumptions in allowing rotation of the polygons, motivating a linear-algebraic approach.

Flexibility analysis and simulation of continuous materials such as cloth [12], string [13, 14], and flexible volumes [15] has a rich history. Models may include finite elements (e.g. [16]), or may be inherently spatially continuous (e.g. Cosserat models [17]). The present work differs in that the component modules are rigid and of irregular shape, requiring explicit consideration of the possible configurations of the chain.

The motions in the present paper allow points and edges to approach, while balancing the rates so as to optimize net motion in some direction. The distance constraints are similar to those used in recent motion planning work [18], as well as in Linear Complementarity Problem (LCP) formulations of dynamics [19, 20, 21] and in study of The Carpenter’s Rule Problem [22].

Linearizing motion around an initial configuration allows for the study of systems of blocks with many thousand degrees of freedom; our approach draws inspiration from early linear grasp analysis techniques [23, 24]. In contrast to manipulability and grasping problems, the blocks which we consider are only loosely connected. Caging grasps [25, 26, 27, 28, 29, 30, 31] study how robot hands may loosely capture an object; the present paper studies motion of structures in which either pairs of blocks or combinations of many blocks may cage each other. Direct construction of configuration spaces of pairs of blocks has a long history; Sacks et al. [32] provides a recent approach, and gives a much higher-fidelity representation of the free motions of small numbers of blocks than our edge/point distance function model. Eckstein et al. [33] analyze how forgiving a connector design is using an approximation of the configuration space of the joint.

Tolerance analysis of mechanical assemblies is utilized in mechanical engineering to determine how frequently small manufacturing errors in the component parts of an assembly will result in unacceptable deviations in the final assembly [34]. The Direct Linearization Method [35] linearizes the homogeneous transformation matrices describing the kinematics of an assembly, and applies statistical techniques to determine what percentage of assemblies are able to be assembled. The Jacobian method and other related methods for tolerance analysis [36, 37] models a mechanical assembly using a set of virtual joints between each element of the kinematic chain representing the assembly.

Once the local configuration space of the chain has been modeled, we solve linear programs to analyze motion; effectively, this is a line search method for numerical optimization [38]. Unlike most algorithms for numerical optimization, our method finds a feasible path through configuration space rather than a single point, since the constraint polyhedron is convex. In the simplex method [39], an $n$-dimensional simplex is constructed that satisfies the constraints and is used as a domain for the next guess in each step, but it is not guaranteed that the path through parameter space taken from the initial guess to the final solution is entirely within feasible space.

Although swarms are not the focus of this work, we briefly explore an example of how the technical approach can be used to find motions for swarms of planar polygon robots. Techniques for robot swarm control typically must handle thousands of simple robots collectively performing some tasks, e.g., object transport [40], shape generation [41], self-assembly [42, 43], and network connectivity [44]; perhaps the closest work in spirit to the present is [45], which controls swarms of robots by allowing robots to bounce off of frictionless walls.

III Linearized distance functions

Let the configuration of the chain be given by $\mathbf{q}\in Q$. Define two types of points of interest: vertices of the polygons describing each body in the chain $\mathbf{o}(\mathbf{q})$ and collision points $\mathbf{p}(\mathbf{q})$. Define a vector of signed distance functions that represents the distance of each collision point from its neighboring edges: $\mathbf{d}(\mathbf{o},\mathbf{p})$. Components of the vector $\mathbf{d}$ will be notated by $d_{i,j}$, where $i$ is the index of the edge and $j$ is the index of the point. To enforce that there are no collisions, $\mathbf{d}(\mathbf{q})\geq 0$. (We abuse the notation to write the distance function in terms of the configuration as both $\mathbf{o}$, $\mathbf{p}$ are functions of $\mathbf{q}$.)

To analyze legal motion and legal nearby configurations of the chain, we may consider the configuration to be a function of time: $\mathbf{q}(t)$. Let $\dot{\mathbf{q}}\in TQ$ be a configuration-space direction indicating possible motion of the system. The instantaneous rate of change of the distance function is

[TABLE]

where $J_{d}$ is the Jacobian of the distance function. For a small enough time step $\Delta t$, an Euler step approximates the change in distances:

[TABLE]

Let $\mathbf{d}_{0}=\mathbf{d}(\mathbf{o}_{0},\mathbf{p}_{0})$ be the distances computed at the initial configuration. We would like to choose motions such that the change in distances from each collision point to each edge does not cause collision: $\Delta\mathbf{d}(t)\leq\mathbf{d}_{0}$. Combining with Equations 1 and 2,

[TABLE]

The scalar $\Delta t$ has been dropped, since we may equivalently linearly scale $\dot{\mathbf{q}}$ and scale time units such that $\Delta t=1$. With this time scaling, the change of $\mathbf{q}$ over a time step is approximated by $\Delta\mathbf{q}=\dot{\mathbf{q}}$. Thus, Equation 3 bounds the change in configuration to a polyhedron.

IV A simple example

Consider a 1R robot arm with base at the origin, and a single link of length 2, shown in Figure 3. The configuration $\mathbf{q}$ is the angle $\theta$; let the initial configuration be $\theta=\pi/4$. Constrain the endpoint of the arm to lie in a square region with vertices $\mathbf{o}=((1,1),(2,1),(2,2),(1,2))$. The end effector coordinates are

[TABLE]

There are four distance functions:

[TABLE]

corresponding to distances from the bottom, right, top, and left walls. Computing the partial derivatives with respect to $\theta$,

[TABLE]

[TABLE]

Candidate boundary values for values for $\dot{\mathbf{q}}$, or equivalently, $\Delta\theta$, are $\approx(-0.29,-0.41,0.41,0.29)$. The value $\Delta\theta=-0.29$ corresponds to collision with the bottom wall, and the value $\Delta\theta=0.29$ corresponds to collision with the left wall; these would be the first collisions to occur. These values are of course approximate, due to the linearization of $J$ around the initial configuration.

V Flexibility analysis using linear programming

We discover approximate extreme configurations of very large 2D systems of loosely connected rigid bodies by solving the linear program

[TABLE]

where $\mathbf{c}$ is a vector of weights. The choice of $\mathbf{c}$ allows us to tune the direction we wish to displace elements.

We use signed distance functions between vertices on one body and edges on another to simply model the permissible local motions of the bodies. For each pair of bodies in the structure, we choose one body to provide the edges, and one body to provide vertices, as shown in Figure 4(a). Since the linearized analysis is only valid for local motions of the bodies, only edges and points that are initially near one another are potential sources of collision; we choose a small positive value $\epsilon$ and select edge/vertex pairs that are initially closer than this value.

Let the configurations of the current pair of bodies under consideration be $\mathbf{q}_{1}=(x_{1},y_{1},\theta_{1})$ and $\mathbf{q}_{2}=(x_{2},y_{2},\theta_{2})$. For each object pair, we expect there to be many distance functions, representing the distances of vertices from edges over some region of near-contact between the bodies. For simplicity, consider a distance function $d_{ij}$ such that object 1 provides the edge, and object 2 provides the vertex. Let $\mathbf{n}$ be the outwards-pointing normal from the edge and $\mathbf{o}$ be the origin. Then

[TABLE]

Let the length of the edge be $\ell$, the first and second endpoints of the edge be $e_{0}$ and $e_{1}$, the distance from the origin of object 1 to the endpoints of the edges be $r_{e_{0}}$ and $r_{e_{1}}$ and the angle from the x axis in the local frame of object 1 to the edge endpoints be $\alpha_{e_{0}}$ and $\alpha_{e_{1}}$. Let the distance from $\mathbf{p}$ to the origin in the local frame of object 2 be $r_{p}$ and the angle from the x axis to $p$ be $\alpha_{p}$. To make the equations more readable, we define the helper variables $s_{e_{1}}=\sin{(\theta_{1}+\alpha_{e_{1}})}$, $c_{e_{1}}=\cos{(\theta_{1}+\alpha_{e_{1}})}$, $s_{e_{0}}=\sin{(\theta_{0}+\alpha_{e_{0}})}$, $c_{e_{0}}=\cos{\theta_{1}+\alpha_{e_{0}}}$, $a=\frac{1}{\ell}$. Then

[TABLE]

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

$\displaystyle\partial_{x_{1}}\mathbf{d}_{ij}$ $\displaystyle=a\left(-r_{e_{0}}s_{e_{0}}+r_{e_{1}}s_{e_{1}}\right)$

$\displaystyle\partial_{y_{1}}\mathbf{d}_{ij}$ $\displaystyle=a\left(c_{e_{0}}r_{e_{0}}-c_{e_{1}}r_{e_{1}}\right)$

$\displaystyle\partial_{\theta_{1}}\mathbf{d}_{ij}$ $\displaystyle=a(c_{e_{0}}r_{e_{0}}\left(c_{e_{0}}r_{e_{0}}-c_{e_{1}}r_{e_{1}}\right)$

$\displaystyle-r_{e_{0}}s_{e_{0}}\left(-r_{e_{0}}s_{e_{0}}+r_{e_{1}}s_{e_{1}}\right)$

$\displaystyle+\left(-c_{e_{0}}r_{e_{0}}+c_{e_{1}}r_{e_{1}}\right)\left(-c_{p}r_{p}+c_{e_{0}}r_{e_{0}}+x_{1}-x_{2}\right)$

$\displaystyle+\left(-r_{e_{0}}s_{e_{0}}+r_{e_{1}}s_{e_{1}}\right)\left(-r_{p}s_{p}+r_{e_{0}}s_{e_{0}}+y_{1}-y_{2}\right))$

$\displaystyle\partial_{x_{2}}\mathbf{d}_{ij}$ $\displaystyle=-a\left(-r_{e_{0}}s_{e_{0}}+r_{e_{1}}s_{e_{1}}\right)$

$\displaystyle\partial_{y_{2}}\mathbf{d}_{ij}$ $\displaystyle=-a\left(c_{e_{0}}r_{e_{0}}-c_{e_{1}}r_{e_{1}}\right)$

$\displaystyle\partial_{\theta_{2}}\mathbf{d}_{ij}$ $\displaystyle=a(-c_{p}r_{p}\left(c_{e_{0}}r_{e_{0}}-c_{e_{1}}r_{e_{1}}\right)$

$\displaystyle+r_{p}s_{p}\left(-r_{e_{0}}s_{e_{0}}+r_{e_{1}}s_{e_{1}}\right))$

V-A Modeling convex corners

The distance-function approximation of the local configuration space is particularly bad for some object geometries. In Figure 4(a), point $\mathbf{p}_{1}$ is closest to point $\mathbf{o}_{1}$, a convex corner. Two distance functions are created, one for each of the extension into lines of the edges $e_{1}$ and $e_{2}$. Maintaining these constraints unnecessarily restricts $\mathbf{p}_{1}$; $\mathbf{p}_{1}$ will remain in the polygonal region defined by the extensions of $e_{1}$ and $e_{2}$. This problem seems fundamental. Rows of the Jacobian express an and relationship; all constraints must be satisfied. But in the example, it is enough that $\mathbf{p}_{1}$ be on the “correct” side of only one of the extended edges.

If only one of the nearby vertices is convex, the problem is easily solvable. For example, in Figure 4(a), points $\mathbf{o}_{2}$ and $\mathbf{p}_{2}$ may be swapped, so that we compute the distance of a point relative to a concave corner. To mitigate the problem in the case where both corners are convex, we may take a simple, though not entirely satisfactory, approach. Take the normals of each edge, and average them, yielding a half-plane constraint that at least allows $\mathbf{p}_{1}$ to cross over the extended edges. One promising avenue for a deeper exploration of this issue is formulation as a Linear Complementarity Problem (LCP), allowing or relationships between constraints.

V-B Time-stepping and re-enforcement of constraints

Solutions to the linear program in Equation 11 are extreme vertices of the constraint polyhedron. Because of the linearization around the initial configuration, the constraints may be violated when the resulting solution is used to compute a new configuration.

A common approach for dealing with truncation error in finite difference methods is to find the net change over several time steps. Although there are sophisticated ways to compute an optimal time step for finite difference methods, for this problem, the cost of computing the linear program solution far outweighs the cost of Euler-step integration and forward kinematics distance computation. We take a simple approach, and do a linear or binary search for a time step, multiplying the displacement vector $\Delta\mathbf{q}$ by an increasingly larger scalar until the maximum distance constraint violation exceeds a user-defined threshold. After a time step is found and applied, a new linear program may be formulated and solved around the new configuration $\mathbf{q}$. Usefully, the new linear program re-enforces the constraints, potentially taking a backward step with $\Delta\mathbf{q}$. This means that error does not accumulate across time steps.

VI Example problems

In this section, we present some informal examples – preliminary work that suggests interesting applications.

VI-A Structure and block design problems

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

As an example, we consider how to add blocks to brace a structure and limit maximum flex. Figure 5 shows an example of adding such a beam to a structure; $\mathbf{c}$ was chosen to maximize radial flex of each block outwards from the center. We find the pair of mutually visible vertices which has changed the most in the predicted configuration of maximum flex, an $O(n^{2})$ operation for $n$ vertices. In this example, this approach suggested adding a vertical cross-beam of blocks, which we did by hand. In a completely automated algorithm, structural limitations of the blocks would need to be taken into account when selecting a cross beam.

The linear programming approach can also be used to explore joint geometry. Looser joints simplify assembly; if the joints in Figure 2 are too tight, the chair cannot be assembled due to limits on the precision of assembly and fabrication. However, if joints are too loose, the structure will flex unacceptably, particularly if there is wear on the connectors over time. Figure 6 (right) shows a planar example of the soda can with loosened joints, with flex computed using linear programming.

One simple strategy to explore joint tolerance is to parameterize the tightness of a joint with a single value and binary search for maximum tolerance. To simulate such a process, we utilize the Clipper library [46] to simulate loosening joints by insetting the boundary of the rigid bodies. A more general approach might choose several parameters to describe joint geometry, and search over this parameter space.

VI-B Flock formations

Figure 7 shows a flock of 1024 robots; the magnified inset shows the geometry. Gray square robots are forbidden from physical collision, and the yellow cone shows a requirement that each robot’s camera must maintain view of a marker (red dot) on the robot in front of it. We can drive the flock into interesting configurations by selecting an objective function. Figure 7 shows an example: 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 robots toward the x value of the leader robot.

We add field-of-view constraints for each robot except the leader, and collision constraints that require that vertices of each robot do not cross the half planes described by the edges of its five nearest robots. We added a constraining square around the leader at the tip of the tree so that the constraint polyhedron is bounded. Large rotations are poorly approximated by the linear method, so we place an arbitrary limit on the rotation displacement of each robot in a time step, using auxiliary linear constraints. After each configuration update, we re-select distance constraints between swarm neighbors.

VI-C Unbounded separation and (dis-)assembly planning

The classic assembly problem [47, 48] is to discover motions that separate or assemble a collection of rigid bodies. For simple versions of this problem, we might like to discover a velocity direction $\dot{\mathbf{q}}$ for which the linear constraints we formulated are unbounded. With some minor modifications, our approach is able to discover such a motion.

Linear program solvers are capable of detecting whether the feasible polyhedron is unbounded in the direction of a given cost vector; in contrast, we would like to discover such a cost vector automatically. Our approach is based on the observation that for almost every non-zero vector, the linear sum of the elements is either positive or negative, but not zero. We may compute the sum of the $x$ and $y$ elements of $\dot{\mathbf{q}}$ by adding a row of the form $(1,1,0,1,1,0,\dots)$ to $J$. We may constrain that sum to be very large, by adding an additional large element $k$ to $\mathbf{d}_{0}$. Choose the objective function $\mathbf{c}$ arbitrarily. We must also upper bound the motion so that the solution is not unbounded; we add a row $(-1,-1,0,\ldots)$ and an element $-2k$ to $\mathbf{d}_{0}$.

If a solution is found to this linear program, then the resulting $\dot{\mathbf{q}}$ removes at least one block far enough from the assembly that it is unconstrained, allowing unbounded motion. If not, then we may look for negative motions by changing the signs on the last two elements of $\mathbf{d}_{0}$. If both of these linear programs are infeasible, then the only separating motions must be such that the sum of the $x$ and $y$ velocity elements is exactly [math]. Our study of this approach is preliminary; Figure 8 shows an example of a direction of separation found.

VII Evaluation and comparisons

The size of the linear program depends on the number of blocks, the complexity of their shape, and the ways in which they are connected; the number of time-step iterations depends on the flexibility of the structure with respect to angular motions in configuration space. While there do not appear to be existing competing methods to solve exactly the problem under consideration, we explore the time costs for various problem sizes to serve as a baseline for future comparison.

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

In Table I we show the result of tests on several systems of rigid bodies of varying size. For each structure, we report the amount of time and number of iterations required for our time stepping procedure to converge. The run time of our approach is dominated by the solution of the constraint Jacobian linear program. In our experiments, we found that certain instances were especially hard for the linear program solver. For instance, the 392 rigid body structure takes five times as long the 396 body structure to solve and two times as long as the 688 body structure. The 392 body structure is a very dense structure, making the placement of each rigid body dependent on a larger number of other rigid bodies than in less dense structures.

VIII Limitations and future work

We presented a simple linear-constraint method for computing the motion of a loosely-connected chain of rigid bodies. Like robot kinematics formulations, the approach is geometric, and does not model dynamics and contact. This is both a strength and a weakness; dynamics simulators may provide realistic motions, but the linear constraints describe a space of possible motion of the system, allowing fast and interesting optimizations. The linear constraint method may also be more useful for a worst-case analysis; just because a simulator provides a trajectory does not mean that trajectory will occur in the real world.

The linear-constraint method assumes that the configuration space is tight enough that linearization of the change in distance functions with respect to configuration-space motion is not too inaccurate. For more flexible systems, the computed motions violate the distance constraints. Repeated enforcement of the constraints by time-stepping and re-solving the linear program gives results that seem empirically reasonable, but there is much to be done to put this approach on firmer mathematical footing, perhaps by analyzing Taylor series approximations [49].

The use of a linear objective function is also limiting. For example, while we can analyze separability of objects (Section VI-C), there is little control over which separating motion is discovered.We might like to separate objects in an assembly one at time (if we have only one robot arm), or simultaneously, for speed; it is unclear how these preferences might be encoded with linear objective functions.

The use of the union of edge-vertex distance constraints to approximate the local configuration space also needs further study; as pointed out in Section V-A, convex corners of objects pose a particular problem when used as edges for the distance function. Extension to 3D, an obvious next step for the work, seems mostly straight-forward, but we expect expressing the geometry of convex vertices, saddles, and ridges using a union of linear constraints to be more problematic than in the 2D case.