PuzzleFlex: kinematic motion of chains with loose joints

06/20/2019 ∙ by Samuel Lensgraf, et al. ∙ 0

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.



There are no comments yet.


page 2

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

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. Figure 

1 shows an example of a planar puzzle flexing in such a way that the upper right block moves maximally in the positive 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?

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

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, but this limitation is not fundamental.

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 [gurobi] 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 [Revels2016-automatic-differentiation-in-julia]. Figure 9 shows a structure with 1703 blocks, with displacement of the upper right block maximized using this implementation.

Ii Related work

Figure 2: Interlocking puzzle blocks, from [Zhang2018-interlocking].

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 work on

manipulability ellipsoids [Chiacchio1991-manipulability, Park1998-manipulability, Kim1998-manipulability, Bicchi2000-manipulability]

, in which directions of motion of the end effector of a robot arm are analyzed at a particular configuration by examining eigenvectors of the Jacobian matrix. Work by Berenson also provides analysis and approximations of Jacobians for truly flexible cloth or string 

[Berenson2013-deformable]. Linear grasp analysis techniques also serve as inspiration. In the 19th century, Reuleaux [Reuleaux1876] derived a geometric method to find the free motion of an object in contact with frictionless fingers. Mishra, Schwartz, and Sharir’s seminal work on the minimum number and sufficient placement of fingers to immobilize an object [Mishra1987-grasp-existence] analyzes polyhedral constraints in twist and wrench spaces.

In contrast to manipulability and grasping problems, the blocks which we consider are only loosely connected. Caging grasps [RodriguezMF12, makita2008, vahedi2008caging, erickson2003capturing, rimon1996caging, allen2015two, Makita2017-caging-survey] 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. [SacksBM17] 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. [Eckenstein2017-acceptance-area-connectors] analyze how forgiving a connector design is using an explicit approximation of the configuration space of the joint.

The Carpenter’s Rule Theorem states that any open polygonal chain (a planar revolute robot arm) can be reconfigured arbitrarily without self-intersection [Connelly2003-carpenters]; the proof uses expansive motions that cause points and edges to separate from one another. The motions in the present paper allow points and edges to approach one another, while balancing the rates so as to optimize net motion in some direction. The distance constraints are similar to those used in Linear Complementarity Problem (LCP) formulations of dynamics [STsiam97, TTPrs01], which have been used both for rigid body simulation and design for manipulation [Balkcom2002b].

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 [Chase1991-survey-of-tolerance-analysis]. The Direct Linearization Method [Chase1996-geometric-tolerance-analysis] 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.

Methods for building modular interlocking structures have been studied by Zhang et al. [Zhang2018-interlocking, Zhang2016a] and by Werfel et al. [Werfel2006-mobile-robot-construction]; however, the structures are assumed to be static after construction with idealized perfect connectors between blocks. Techniques for robot swarm control typically must handle thousands of simple robots collectively performing some tasks, e.g., object transports [alonso2017multi], shape generation [hsieh2008decentralized], self-assembly [o2014self, rubenstein2014programmable], and network connectivity [esposito2006maintaining]; perhaps the closest work in spirit to the present is [ShahrokhiMB17], 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 . Define two types of points of interest: vertices of the polygons describing each body in the chain and collision points

. Define a vector of signed distance functions that represents the distance of each collision point from its neighboring edges:

. Components of the vector will be notated by , where is the index of the edge and is the index of the point. To enforce that there are no collisions, .

To analyze legal motion and legal nearby configurations of the chain, we may consider the configuration to be a function of time: . Let be a configuration-space direction indicating possible motion of the system. The instantaneous rate of change of the distance function is


where is the Jacobian of the distance function. For a small enough time step , an Euler step approximates the change in distances:


Let be the distances computed at the initial configuration of the chain. We would like to choose motions such that the change in distances from each collision point to each edge does not cause a collision: . Combining with Equations 1 and 2,


The scalar has been dropped, since we may equivalently linearly scale and scale time units such that . With this time scaling, the change of over a time step is approximated by . Thus, Equation 3 bounds the change in configuration to a polyhedron.

Iv A simple example

Figure 3: 1R planar arm, and estimates of end-effector collisions.

Consider a 1R robot arm with base at the origin, and a single link of length 2, shown in Figure 3. The configuration is the angle ; let the initial configuration be . Constrain the endpoint of the arm to lie in a square region with vertices . The end effector coordinates are


There are four distance functions:


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


Candidate values for , or equivalently, , are then


The value corresponds to collision with the bottom wall, and the value 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 around the initial configuration.

V Flexibility analysis using linear programming

We apply the concepts developed above to approximating extreme configurations of very large 2D systems of loosely connected rigid bodies by solving the linear program

subject to

where is a vector of weights. The choice of allows us to tune the direction we wish to displace elements of the chain.

(a) The edge-vertex distance constraints limit the valid motions in a small convex region.
(b) The distance constraints generated for a pair of rigid bodies. Blue line segments are vectors from green vertices to red edges.
Figure 4: Local distance functions model the free space.

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 3(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 and select edge/vertex pairs that are initially closer than this value.

Let the configurations of the current pair of bodies under consideration be and . 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 such that object 1 provides the edge, and object 2 provides the vertex. Let be the outwards-pointing normal from the edge and be the origin. Then


Let the length of the edge be , the first and second endpoints of the edge be and , the distance from the origin of object 1 to the endpoints of the edges be and and the angle from the x axis in the local frame of object 1 to the edge endpoints be and . Let the distance from to the origin in the local frame of object 2 be and the angle from the x axis to be . To make the equations more readable, we define the helper variables , , , , . Then


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:

V-a Modeling convex corners

The distance-function approximation of the local configuration space is particularly bad for some object geometries. In Figure 3(a), point is closest to point , a convex corner. Two distance functions are created, one for each of the extension into lines of the edges and . Maintaining these constraints unnecessarily restricts ; will remain in the polygonal region defined by the extensions of and . 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 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 3(a), points and 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 to cross over the extended edges. Once an edge has been crossed, it is no longer treated as a valid constraint and dropped from the Jacobian, allowing a second time-step to more accurately model the motion of . Deeper exploration of this issue is a primary goal of future work; one promising avenue 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 12 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. The linearized distance functions are Taylor series approximations, truncated after the first term. 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 by an increasingly large 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 .

Usefully, the new linear program re-enforces the constraints, potentially taking a backward step with . This means that error does not accumulate across time steps, and also suggests an even more computationally efficient, though numerically riskier, approach: compute a single-step solution that violates the constraints, and then solve just one more linear program. In future work, we expect to analytically and empirically explore circumstances under which this faster method is sufficient.

Vi Example problems

In this section, we present some informal examples – preliminary work that suggests a breadth of 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.

Figure 5: Example of adding a cross beam into a structure at a point of maximum flex.

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; 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 operation for 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.

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

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 [Johnson2014-clipper-library] 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. A key question is how to choose to test flex; Subsection VI-C shows how to discover such a direction in the extreme case that joints separate and break the structure.

Vi-B Flock formations

Figure 7: A linear programming solution for a flock of 1024 robots which must maintain sensor contact squeezing together to fit through a doorway or hallway.

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

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

The classic assembly problem [HalperinLW00, SnoeyinkS94] 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 for which the linear constraints we formulated are unbounded. With some minor modifications, our approach is able to discover such a direction, thus testing whether a structure is interlocked under constant-velocity motion, which may include rotation.

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 and elements of by adding a row of the form to . We may constrain that sum to be very large, by adding an additional large element to . Choose the objective function arbitrarily. We must also upper bound the motion so that the solution is not unbounded; we add a row and an element to .

If a solution is found to this linear program, then the resulting 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 . If both of these linear programs are infeasible, then the only separating motions must be such that the sum of the and velocity elements is exactly . We neglect this case in our implementation, but perhaps it might be handled if desired by rotating the entire structure slightly, changing the relationship between and velocities along the separation direction.

Our study of this approach is preliminary; issues of numerical stability may arise that we have not discovered. We have explored a few examples; Figure 8 shows an example of a direction of separation found by this method. As will be discussed in the limitations section below, this approach gives little control over which direction of separation is selected. Future work may explore non-linear objective functions or direct analysis of the constraint polyhedron, perhaps by computing extreme edges, the double description problem [Fukuda96doubledescription].

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. In this section, we explore the time costs, evaluated in terms of the size of the linear programs, number of steps, and the practical run-time on a current desktop system.

For blocks with one block held fixed, the Jacobian has columns and rows, if 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 non-zero entries in the matrix. We omit formal asymptotic run-time analysis of the solution, since the solution techniques are standard for linear programming.

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
Table I: Performance results for several structures, using Gurobi with sparse matrices.
Figure 9: Structure composed of 1703 rigid bodies flexing upwards. Red polygons denote the initial configuration.

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

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

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.

We have begun to conduct a comparison to state-of-the-art dynamics simulation approaches, using external forces applied to the rigid bodies as a loose parallel to the objective function used for the linear program. We have simulated a simple beam of 50 blocks, and solutions are similar. However, it is not immediately clear how to choose these external forces for dynamic simulation of more complex structures, since we expect that for some systems, counter-intuitive motions of some blocks will lead to maximized flex. Computational costs are difficult to compare; for a dynamic simulator, reaching a flexed configuration requires many time steps through impact events, but for the linear program, many possible alternative motions are analyzed to maximize the objective. Future work will attempt to make more direct comparisons.

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 structures that are nearly rigid, as we might like to build in assembly problems, this assumption is reasonable, and the one-step solution to the linear program gives good results. However, 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 of the change in distance functions [Duistermaat2010-taylor-polynomial-many-variables].

The use of a linear objective function is also limiting. Figure 10 shows an example of a rectangular robot in a square room. Attempting to maximize rotates the robot from 1, to 2, to 3, to 4, and to 5 in successive time steps, but because is unbounded, time-stepping will not converge. Other interesting problems are also an imperfect match for linear objective functions. While we can analyze separability of objects using the approach outlined in Section VI-C, there is little control over which separating motion is selected by the linear program. 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.