CUIK++: An Extension of Branch-and-Prune Techniques for Motion Analysis and Synthesis of Complex Robotic Systems



Methods


[Brach-and-prune]    [Higher-dimensional continuation]


Brach-and-prune

Historically, the preferred approach to tackle the position analysis problems has been to reduce the initial system of equations encoding the problem to a resultant polynomial, and then solving this polynomial using well-established methods for the univariate case. However, this approach may introduce extraneous roots and the size and degree of the resultant grow rapidly with the amount of intervening bodies and the complexity of their connection pattern. Our approach circumvents these issues by adopting an opposite approach. Instead of reducing the initial system of equations to a univariate polynomial, we formulate it as a larger system including only linear and quadratic equations. This particular formulation is exploited to implement a branch-and-prune technique that, departing from an initial box bounding the solution set, effectively prunes unfeasible portions of such box until all solutions are isolated at the desired accuracy.

A simple example can illustrate how the branch-and-prune methods work. Consider the following planar manipulator



A planar arm welding on a beam.

A planar arm welding on a beam. The goal is to weld on a particular point P with orientation θ. Click on the image to enlarge it.



The problem can be encoded in the following set of equations:



The equations encoding the welding problem.

The equations encoding the welding problem. Click on the image to enlarge it.



Observe that the arm configurations are represented using three normalized vectors, (u1_x,u1_y), (u2_x,u2_y), and (u3_x,u3_y), encoding the orientation of each link relative to the X axis of the global frame. The position of the end effector is given by the linear equations in the last two lines of the system, and the [-1,1] ranges for the variables define a six-dimensional box bounding the location of all solutions to the problem.

By means of simple manipulations, the equations can be simplified to the following couple of equations, whose solution are in the intersection of a line and a circle in u2_x and u2_y



The simplified equations encoding the welding problem.

The simplified equations encoding the welding problem. Click on the image to enlarge it.



and the search box can be limited to the ranges [-0.75, 1] and [-0.25, 1] in u2_x and u2_y, respectively. The graph of these two equations and the new search box are represented next using solid and dotted lines:



The bisection and prune process to isolate solutions.

The bisection and prune process that allows isolating the solution points. Click on the image to enlarge it.



To identify the two solution points, the box can be bisected along the u2_x axis, obtaining the two yellow boxes in the bottom of the figure. Then, a linear program taking into account only the line equation can be used to reduce these sub-boxes to the light-gray areas shown in the figures. However, the circle equation can also be considered by using two inequalities (the dashed lines in the figures), which bound the graph of the circle equation within the corresponding search box. Using these inequalities, a tiny box around the first solution point is readily identified, and tighter bounds are provided for the second solution point, producing the dark-gray rectangle in the figure. This pruning operation can be repeated and at the end, if a box is small enough, i.e., if its largest side is below a given threshold, it is considered a solution box. Otherwise the box is bisected and the process is recursively applied to the two sub-boxes created. This process has proved to be successful on solving complex problems in general 6R robots and Stewart platforms, or in mechanisms with up to sixteen highly-coupled closed kinematic chains. Alternative methods are finding their limit in mechanisms of much smaller size



Higher-dimensional continuation

The branch-and-prune methods are complete, in the sense that they can isolate all the configurations, irrespectively of whether they form one or several connected components. In many problems, however, it may be sufficient to explore only those configurations that are path-connected. To this end, we developed higher-dimensional continuation tools allowing to trace arbitrary manifolds. While branch-and-prune methods proceed by discarding non-valid configurations, continuation techniques march from a given point in all directions identifying neighboring valid configurations. Local charts of the C-space are constructed and mutually coordinated along the way, defining a global atlas that is suitable to determine feasible motion ranges, optimal configurations, or paths connecting configurations.

Continuation methods generate atlases of the C-space regions that are connected to a given point. To see how such an atlas is constructed, let's assume that F(x)=0 is any system of equations encoding all the loop-closure constraints of the multibody system, whose solution set constitutes the C-space C under consideration, and let xi be an initial configuration satisfying F(xi)=0. The points on the tangent space of C at xi, Ti, can be parametrized by

xji = xi + Φi uji

where Φi is a matrix providing an orthonormal basis of Ti, and uji is a parameter vector with the same dimension as C. By choosing a value for uji we obtain a point xjiTi, which can be projected down to C by solving the system

Equations projecting from tangent space to the manifold.

which provides the point xjC lying in the normal line through xji

A chart is used to obtain new C-space points by projecting points from the tangent space.

A chart is used to obtain new C-space points by projecting points from the tangent space. Click on the image to enlarge it.

The new configuration xj can be used to define a new chart that can be coordinated with the previous chart

When a new chart is defined, the chart domains are mutually cropped to avoid overlaps in the local parametrizations.

When a new chart is defined, the chart domains are mutually cropped to avoid overlaps in the local parametrizations. Click on the image to enlarge it.

At the of the process, the whole component of C reachable from xi gets fully covered:

Progress of the atlas construction method.

Progress of the atlas construction method. The C-space is shown projected in three of the problem variables. Red polygons represent the charts to be extended in subsequent iterations. Click on the image to enlarge it.

Every time a new chart is defined at a point, we check for the presence of bifurcations of C, and propagates the atlas construction through such bifurcations in order not to leave areas unexplored.