Starting at an immobilizing grasp ofB, the caging algorithm searches the augmented caging graph, GT, using two lists of nodes: a list of open nodes, O, and a list of closed nodes, X. The open list
is kept sorted in ascending order of the associated value of dps1, s2q. The closed listis kept in the
order by which each node was added to this list. To start, the node ofGT associated with the initial
immobilizing grasp is marked as open and inserted intoO, whileX is initially empty. At each step, the node inOwith the smallest value ofdps1, s2qis marked ascurrent, removed fromO, and added
to theendof the closed listX. If thedps1, s2qvalue of the current node is zero, the algorithm halts,
as an escape point has been found. Otherwise, each node adjacent to the current node inG, if not inX and not already inO, is added to O.
The algorithm halts after a finite number of steps, since there is always a maximal puncture point grasp (corresponding to a node ofGT), beyond which an escape point (corresponding to an
escape node ofGT) can be reached. The algorithm’s output is extracted from the closed listX as
discussed below. In particular, the node inX having the maximal value ofdps1, s2qis themaximal
puncture point grasp associated with the initial immobilizing grasp. A pseudo-code of the caging algorithm follows.
Algorithm 2.1 Contact Space Caging Algorithm Data structures: open listO, closed list X. Initialize: O= initial immobilizing grasp,X H.
whiledpCurrentN odeq ¡0 do
SetCurrentN ode= lowestdps1, s2qvalue node inO.
MarkCurrentN odeas explored.
ExpandCurrentN ode(add all non-explored adjacent nodes inGT toO).
RemoveCurrentN odefromO. AddCurrentN odeto end ofX.
end while
return list of closed nodesX.
The caging algorithm runs on the augmented caging graph,GT, which requires that tunnel curve
edges be added to the caging graph G. These edges are added toG as follows. Consider a non- feasible local minimum ofdps1, s2q, at which finger 1 lies at p1 and finger 2 lies atp2. The node of
Gcorresponding to this grasp is the start point of the tunnel curve. Assume that finger 1 is able to move towardsp2without penetratingB(at every non-feasible local minima one or both fingers may
be moved towards the other finger without penetratingB). Find the intersection of the line segment joining p1 to p2 with all edges ofB. If no such intersection exists, the tunnel curve’s end point is
located at an escape node ofG, where both fingers lie atp2. Otherwise, find the intersection which
is closest top1. Generically, the grasp in which finger 1 lies at this intersection while finger 2 lies at
p2is located in theinteriorof a contact space rectangle. The tunnel curve’s end point is located at
the node ofGon the boundary of this contact space rectangle, having the minimumdps1, s2qvalue
in this rectangle. The procedure is summarized in Algorithm 2.2.
Algorithm 2.2 Contact Space Tunnel Curve Construction Input: caging graph,G.
Initialize: GT =G.
foreach non-feasible local minimum,vnpp1, p2qPG,do
Set tunnel curve start node,vsvn.
Define line segmentl running fromp1 top2.
Find all intersections ofl with edges ofB.
if no such intersection point existsthen
set tunnel curve’s end node,ve pp2, p2q.
else
pint = intersection point closest top1,
Rij = contact space rectangle containingppint, p2q,
ve= minimum inter-finger distance node in Rij.
Set tunnel curve’s end node atve.
end if
Add edge fromvs tove toGT.
end for
return GT, the augmented caging graph.
Interpretation of the closed list: Consider the nodes v P X of the closed list, indexed in the order they were added toX. The first node added (the initial immobilizing grasp), is denoted v0,
and so on.
Definition 2.7.1 The ith node, v
i, in the closed list X is a local minimum in X if dpviq
mintdpvi1q, dpvi 1qu.
Definition 2.7.2 The ith node, v
i, in the closed list X is a local maximum in X if dpviq ¡
maxtdpvi1q, dpvi 1qu.
Definition 2.7.3 The ith node, v
i, in the closed listX is apuncture-related local maximum
if:
1. It is a local maximum inX.
2. There does not exist a lower indexed puncture-related local maximum in X, vj forj i, such
that dpvjq ¡dpviq
(i.e. it is the highest value node so far in X).
The interpretation of the closed list X is simple. The first element ofX, by construction, and all local minima are immobilizing grasps of B. All puncture-related local maxima in X are puncture- point grasps ofB. While the node representing each of these punctures is a saddle ofdps1, s2q in
U, it appears as a local maximum in X. This occurs because once the search algorithm discovers a saddle, it continues searching lower valued nodes in the unexplored portion of the previously disjoint sublevel set of the augmented caging graph, GT. The lowest indexed puncture-related
local maximum in X represents the puncture-point grasp of the local caging set surrounding the immobilizing grasp. The highest indexed puncture-related local maximum in X represents the maximal puncture-point grasp of the maximal caging set, beyond which the objectB can escape to infinity. Finally, all other puncture-related local maxima represent puncture-point grasps associated with all intermediate caging sets surrounding the immobilizing grasp.
Computational complexity: Let the object B have n vertices and edges. Each vertex of B induces at most 2n nodes (n vertex-vertex pairs and n vertex-edge pairs) in the caging graph G. Hence there are Opn2q nodes inG. The open list, O, contains at most Opn2q nodes, which may be kept sorted inOpn2lognq time. Additionally, the object B may have up toOpn2qnon-feasible equilibrium grasps that may induce tunnel curves. The construction of Opn2qtunnel curves may
be done in Opn2 log nq time using ray shooting algorithms. Based on these considerations, the
caging algorithm runs inOpn2lognqtime. Additionally, while the algorithm searches for the initial,
intermediate, and maximal caging sets surrounding an initial immobilizing grasp, it can be adapted to find the maximal caging sets surroundingallimmobilizing grasps ofB, also inOpn2 log nqtime,
which is comparable to [27, 43].