Ray-Shooting Algorithms for Robotics · Ray-Shooting Algorithms for Robotics 5 (a) (b) Fig. 3 Illustration of the GJK-based ray-shooting algorithm in 2-D space. (a) Iteration. b0

Ray-Shooting Algorithms for Robotics

Yu Zheng and Katsu Yamane

Abstract Ray shooting is a well-studied problem in computer graphics. It also oc-curs in robotics as a collision detection problem in 3-D object space or a contactforce optimization problem in 6-D wrench space. However, the ray-shooting algo-rithms derived in computer graphics are limited to 3-D polyhedra and not suited forgeneral convex sets in high-dimensional space. This paper discusses several generalray-shooting algorithms and their applications to these problems in robotics.

1 Introduction

In computational geometry and computer graphics, the ray-shooting problem dealswith computing the first intersection point on the surface of given objects by a queryray and has been well studied for over four decades [1, 8, 13, 14]. In these efforts,most of the practical algorithms are limited to 2-D or 3-D polytopes.

It has been discovered that several fundamental problems in robotics are equiv-alent to a ray-shooting problem. Ong and Gilbert [10] proposed a unified distancemeasure, called the growth distance, for both separated and penetrated objects andapplied it to collision-free path planning [11]. The growth distance computationcan be reduced to a ray-shooting problem. Liu et al. [6, 2, 7] cast several problemsin grasping, such as grasping force optimization and force-closure grasp test andsynthesis, into a ray-shooting problem. However, these ray-shooting problems dealwith the intersection of a ray with a set, which is specified by nonlinear paramet-ric functions combined with complex operations on sets, such as the Minkowskisum, rather than a 3-D polytope with given vertices or facets. Some of them arealso in higher-dimensional space than 3-D. Therefore, the ray-shooting techniquesin computational geometry and computer graphics are not suited here. The previ-ous solutions to these problems relied on general-purpose optimization techniques

Yu Zheng, Katsu YamaneDisney Research Pittsburgh, USA, e-mail: {yu.zheng, kyamane}@disneyresearch.com

1

2 Yu Zheng and Katsu Yamane

[10, 6] and suffered the low computational efficiency. Recently, two procedures wereproposed to more quickly solve such a ray-shooting problem [16, 18].

In this paper, we extensively discuss the algorithms for a general ray-shootingproblem and their applications in robotics. The original contributions include:

• A better stopping criterion for the algorithm [16] to enhance its accuracy.• Generalization of the algorithm [18] to any case with guaranteed convergence.• Discussion on a new algorithm and hybrid uses of these algorithms to gain higher

computational efficiency.• Application of these ray-shooting algorithms to growth distance computation, in

addition to contact force optimization in the previous work [16, 18].

The rest of this paper is organized as follows. Sect. 2 defines the ray-shootingproblem and summarizes algorithms to be used in developing ray-shooting algo-rithms in Sect. 3. Sects. 4 and 5 show their applications with numerical examples.Conclusions and future work are included in Sect. 6. In the following discussion,we will use many convex geometry concepts, for which we refer readers to [4].

2 Problem Statement and Preliminaries

In this section, we give a mathematical definition of the ray-shooting problem andsummarize two existing algorithms for minimum distances [3, 15].

2.1 Definition of the Ray-Shooting Problem

Let A be a compact convex set with nonempty interior in Rn, r a nonzero vector inRn, and R(r) the ray emanating from the origin 0 of Rn in the direction r, i.e.,

R(r),{

λr ∈ Rn | λ ≥ 0}. (1)

The ray-shooting problem first needs to determine

1. whether A and R(r) intersect.

If A and R(r) intersect, then it is also aimed at computing

2. the farthest intersection point zA(r) of A with R(r) from the origin 0;3. a set of affinely independent points in A, denoted by ZA(r), such that zA(r) can

be written as a convex combination of ZA(r);4. the normal n of the hyperplane that passes through zA(r) and supports A.

In the algorithms discussed later to solve the ray-shooting problem, the supportfunction hA and the support mapping sA of A are often used, which are defined by

hA(u), maxa∈A

uT a, sA(u), argmaxa∈A

uT a. (2)

Ray-Shooting Algorithms for Robotics 3

(a)

b HH'

s( ) n

kvk

T

(b)

Fig. 1 Illustration of the GJK algorithm in 2-D space. (a) Iteration that leads vk to vA(b). (b)Stopping criterion hA(nk)−nT

k vk < εGJK. H and H ′ are hyperplanes with normal nk passing vk andsA(nk), respectively, and bounds vA(b) in the gap between them with the width of hA(nk)−nT

k vk.

2.2 Summary of the GJK Algorithm [3]

Let b be an arbitrary point in Rn. The purpose of this algorithm is to compute thedistance dA(b) between b and A and the closest point vA(b) in A to b, defined by

dA(b), mina∈A

∥a−b∥, vA(b), argmina∈A

∥a−b∥. (3)

Since A is compact and convex, vA(b) exists and is unique. If b /∈ A, then dA(b)> 0and the hyperplane H with normal b− vA(b) passing through vA(b) supports A atvA(b), as depicted in Fig. 1a; otherwise dA(b) = 0 and vA(b) = b.

Both dA(b) and vA(b) can be computed by the GJK algorithm, as illustrated inFig. 1. It starts with any affinely independent set V0 in A and iterates by Vk+1 =Vk ∪{sA(nk)}, where Vk is a minimal subset of Vk to represent vk (the closest pointin CH(Vk) to b) as its convex combination and nk is the unit vector from vk tob. If hA(nk) > nT

k vk, then dCH(Vk+1)(b) < dCH(Vk)(b). Hence, dCH(Vk)(b) is strictlydecreasing with the iteration and converges to dA(b) while hA(nk)−nT

k vk < εGJK,where εGJK is the termination tolerance. Then, vk converges to vA(b) and Vk givesan affinely independent set in A, denoted by VA(b), to represent vA(b) as its convexcombination. If b ∈ A, then vA(b) = b and b is a convex combination of VA(b).

2.3 Summary of the ZC Algorithm [15]

Let CO(A) denote the convex cone of A [4], which consists of all nonnegative com-binations of A. This algorithm computes the distance dCO(A)(b) and the closest pointvCO(A)(b) between b and CO(A), defined similarly to (3) with A replaced by CO(A),as illustrated in Fig. 2. It starts with a linearly independent set V0 in A and iteratesby Vk+1 = Vk ∪{sA(nk)}, where Vk is a minimal subset of Vk to represent vk (the


Fig. 2 Illustration of the ZC algorithm in 3-D space. If b ∈ CO(A), then a linearly independentsubset of A, namely V1 here, is obtained to contain b in its convex cone.

closest point in CO(Vk) to b) as its positive combination and nk is the unit vec-tor from vk to b. This iteration leads dCO(Vk)(b) to dCO(A)(b) and vk to vCO(A)(b)while hA(nk) < εZC, where εZC is the termination tolerance. The final Vk providesa linearly independent set in A, denoted by VCO(A)(b), to represent vCO(A)(b) as itspositive combination. If b ∈ CO(A), then dCO(A)(b) = 0 and vCO(A)(b) = b, and bis a positive combination of VCO(A)(b).

3 Ray-Shooting Algorithms

In this section, we discuss procedures to solve the ray-shooting problem and theirhybrid uses for higher efficiency. In general, each procedure generates a sequenceof points on R(r) approaching zA(r). Hence, the convergence of these procedurescan be easily proved by using the monotone-convergence principle [12].

3.1 A GJK-Based Procedure

A procedure based on the GJK algorithm to solve the ray-shooting problem wasproposed in [14, 16, 17]. It computes zA(r) by iterating a point bk on R(r) outsideof A, as depicted in Fig. 3a. Initially, b0 can be taken to be hA(r)r/rT r, which is theintersection point of the supporting hyperplane H0 = H(r,sA(r)) of A with R(r) andnot in the interior of A. Then, we update the point bk by

bk+1 = bk −dA(bk)

rT nkr (4)

where dA(bk) and vA(bk) are the minimum distance and the closest point from A tobk, respectively, which are computed by the GJK algorithm, and nk is the unit vector


(a) (b)

Fig. 3 Illustration of the GJK-based ray-shooting algorithm in 2-D space. (a) Iteration. b0 is theintersection point of H0 with R(r). H1 is the hyperplane with normal b0 −vA(b0) passing throughvA(b0) and intersects R(r) at b1, which is closer to zA(r) than b0. (b) Stopping criterion. Upper:Although dA(b1) is small, this does not guarantee that b1 is close enough to and can be taken tobe zA(r). Lower: A better stopping criterion is that R(r) intersects CH(VA(b2)) (the green linesegment). Then, their intersection point b3 can be adopted as zA(r) and VA(b2) as ZA(r).

from vA(bk) to bk. The iteration (4) can be terminated with the conclusion that R(r)does not intersect A if rT bk < max{−hA(−r),0}, or rT nk = 0 while dA(bk) = 0, orrT nk < 0 [17]. Or else, dA(bk) will decrease to zero and bk will approach zA(r).

Instead of the stopping criterion dA(bk) < ε used in [14, 16, 17], we verifywhether R(r) intersects CH(VA(bk)) or r is a nonnegative combination of VA(bk), asexplained in Fig. 3b. If so, we can compute their intersection point, which is equalto bk+1 from (4) and contained in A, since CH(VA(bk)) is contained in the hyper-plane Hk+1 = H(nk,vA(bk)). The hyperplane H ′

k+1 = H(nk,sA(nk)) supports A andintersects R(r) at the point b′

k+1 = hA(nk)r/rT nk, which is outside of A. It is evi-dent that zA(r) is between b′

k+1 and bk+1. The GJK algorithm stops iterating whenhA(nk)−nT

k vA(bk) < εGJK, which implies nTk (b

′k+1 −bk+1) < εGJK. From this we

can derive ∥zA(r)− bk+1∥ ≤ ∥b′k+1 − bk+1∥ < εGJK∥r∥/rT nk. Therefore, we take

zA(r) = bk+1 and ZA(r) =VA(bk), as depicted in the lower figure of Fig. 3b.

3.2 An Internal Expanding (IE) Procedure

A preliminary version of this procedure was proposed in [18]. However, its con-vergence was unclear in some rare singular situations. Here we solve this issue andpresent a generalized version with guaranteed convergence.

Assume that we have a facet F0 in A such that R(r) passes through its interior. LetFk be a collection of facets in A and initially F0 = {F0}. In the following iteration,Fk may comprise more than one facet, but R(r) intersects every facet in Fk at thesame point, which is on the face CH(Sk) shared by those facets and denoted by bk,


F

R( )r

sA( )n0b1==b2

F2F3

(a)

R( )rsA( )n1

F1b0

(b)

Fig. 4 Illustration of the IE ray-shooting algorithm in 3-D space. (a) F0 = {F0} and R(r) passesthrough an interior point b0 of F0. Then, F+

0 = F0. Assume that R(r) happens to pass sA(n0). Then,F1 = {F1,F2,F3}. Suppose that F2 is the facet in F1 closest to the origin and sA(n2) lies on thedifferent side of F2 or F3 from the origin. Then, F+

1 = {F2,F3} and F4 and F5 are generated toreplace F2 and F3. As a result, F2 = {F1,F4,F5}. (b) Suppose that F1 is the facet in F2 closest to theorigin and sA(n1) lies on the different side of any facet in F2 from the origin. Then, R(r) intersectsa new face, i.e., F6, at a point b3 closer to zA(r) than b2. As b3 is in the interior of F6, F3 = {F6}.

where Sk is the set of vertices of the face. Let ni be the unit normal to each facetin Fk such that nT

i r > 0. Then nTi bk is the distance from the origin to facet i in Fk.

We find facet i∗ in Fk whose distance from the origin is the minimum and computehA(ni∗) and sA(ni∗). If hA(ni∗)−nT

i∗bk > ε , we extract a sub-collection F+k from Fk

such that nTi sA(ni∗) > nT

i bk for any facet in F+k , which implies that sA(ni∗) lies on

the different side of the facet from the origin. Let R be the collection of facets of allfacets in F+

k , which are ridges in Rn, and Ri the collection of facets of a facet in F+k

that contain CH(Sk). Then, we will meet two situations, as depicted in Fig. 4a:

1. F+k is equal to Fk: In this case, sA(ni∗) lies on the different side of every facet in

Fk from the origin. Then, R(r) intersects one of the facets formed by each ridgein R\∪iRi with the point sA(ni∗) and the intersection point, which is assigned tobk+1, is farther from the origin than bk. Hence, we reconstruct the facet collectionFk+1 with the facets intersected by R(r) and the ridge collections Ri for eachfacet in Fk+1 accordingly. Note that R(r) may pass through a common face ofsome of the newly formed facets. In that case, Fk+1 consists of all such facetsand Sk+1 consists of vertices of the face.

2. F+k is a proper sub-collection of Fk: In this case, we construct Fk+1 as follows.

First, Fk+1 contains Fk \F+k . Besides, for each ridge in Ri that is not contained

in other Ri or shared by other facets in F+k , we add the facet formed by the ridge

with the point sA(ni∗) to Fk+1. After doing this for every ridge collection Ri, weobtain a new facet collection Fk+1. However, bk+1 and Sk+1 remain the same asbk and Sk, respectively.

By the iteration in case 2, though bk and Sk do not change, the minimum distancefrom the origin to the facets in Fk is increased. On the other hand, the minimumdistance is bounded above by the distance from the origin to the common face sharedby the facets in Fk. Hence, by repeating this iteration, case 1 will become true, andbk will be updated with a new point closer to zA(r) (see Fig. 4b). As bk approacheszA(r), the stopping condition hA(ni∗)−nT

i∗bk < ε will reach, where i∗ indicates the


(a)

h ( )v

A ( )rbCO( )2

H

'

VCH( 2)

zA( )r

r

b

sA( )n3

b2

n3

n3A b2

(b)

Fig. 5 Illustration of the ZC-based algorithm for ray-shooting in 2-D space. (a) It computes asubset Vk of A in every iteration such that r is a positive combination of Vk −bk−1. Then bk is theintersection point of CH(Vk) with R(r) and approaches zA(r) as the iteration proceeds. (b) Theiteration can be stopped by hA−bk (nk+1) < εZC, where nk+1 is the unit vector from vCO(A−bk)(r)to r. Then the point bk and the hyperplane H with normal nk+1 passing through sA(nk+1) boundszA(r) between them, which implies that bk is close enough to zA(r).

facet in Fk closest to the origin. Then, we can derive ∥zA(r)−bk∥< ε∥r∥/nTi∗r, and

the hyperplane H(ni∗ ,bk) supports A at zA(r) = bk. This procedure can handle thesituation that R(r) passes through a face of dimension lower than n−1 with ensuredconvergence and generalizes the algorithm proposed in [18].

3.3 A ZC-Based Procedure

We propose a novel procedure based on the ZC algorithm, which iterates a point inA towards zA(r). We first let b0 = 0 and compute dCO(A)(r) using the ZC algorithm.Then, R(r) intersects A if and only if r ∈ CO(A) or equivalently dCO(A)(r) = 0.

If R(r) intersects A, the ZC algorithm gives dCO(A−bk)(r) = 0 and a set of linearlyindependent points in A−bk, denoted by a1 −bk,a2 −bk, . . . ,aL −bk, such that

r =L

∑l=1

cl(al −bk) (5)

where al ∈ A and cl > 0 for all l. Let Vk = {a1,a2, . . . ,aL}, σ = ∑Ll=1 cl , and

bk+1 = bk +1σ

r =L

∑l=1

cl

σal . (6)

From (6) it follows that bk+1 is a point on R(r) and it is also a convex combinationof Vk, which implies that bk+1 ∈ A∩R(r), as depicted in Fig. 5a. Since σ > 0, bk+1


is strictly farther from the origin and strictly closer to zA(r) than bk. By repeatingthis iteration, bk+1 will converge to zA(r) and Vk will become ZA(r).

As bk approaches zA(r) and the boundary of A, the ZC algorithm in a cer-tain iteration will terminate with hA−bk(nk+1) < εZC and dCO(A−bk)(r) > 0, wherenk+1 is the unit vector from vCO(A−bk)(r) to r, as depicted in Fig. 5b. The hyper-plane H(nk+1,sA(nk+1)) supports A and intersects R(r) at b′ = hA(nk+1)r/rT nk+1.Then, b′ and bk bound zA(r) between them and nT

k+1(b′−bk) = hA−bk(nk+1)< εZC.

Therefore, we can derive ∥zA(r)−bk∥ ≤ ∥b′−bk∥< εZC∥r∥/rT nk+1. Furthermore,H(nk+1,bk) can be regarded as a hyperplane of support to A at zA(r) = bk.

3.4 Hybrid Uses

3.4.1 A Bi-GJK Procedure

From Sect. 3.3, we know that whether R(r) intersects A can be easily determinedby the ZC algorithm and, if so, a point in A∩R(r) can be obtained, as depicted byb1 in Fig. 5a, which we denote by b here. On the other hand, b = hA(r)r/rT r isa point on R(r) outside the interior of A, as depicted by b0 in Fig. 3a. Apparently,zA(r) lies on the line segment between b and b, which allows us to use the bisectionmethod to compute zA(r). We call the GJK algorithm to compute the minimumdistance dA(b) between the midpoint b = (b+ b)/2 and A. If dA(b)> 0, i.e., b /∈ A,then we can update b with the point from (4) by setting bk = b and nk = (b −vA(b))/∥b−vA(b)∥, which is a point on R(r) closer to zA(r) than b but still outsideof A. Otherwise, we can replace b by b. This procedure can stop as the GJK-basedprocedure in Sect. 3.1, as shown in Fig. 3b, or once ∥b− b∥< ε .

3.4.2 A ZC-IE Procedure

The IE procedure can be much faster than the other two procedures if R(r) alwayspasses through the interior of a new facet in every iteration. In that case, the onlycomputation in an iteration is to determine the facet intersected by R(r) among nnew facets and record its vertices. By contrast, the GJK-based or ZC-based pro-cedure needs to run the GJK or ZC algorithm in every iteration, whose computa-tion cost is much higher. However, there is no guarantee that R(r) can always passthrough the interior of a facet. In case that R(r) happens to pass through only alow-dimensional face shared by several facets, we have to compute and maintain afacet collection and a ridge collection for each facet as discussed in Sect. 3.2, whichincreases the computation cost of every iteration. To keep a relatively lower compu-tation cost, instead we can call the ZC-based iteration as discussed in Sect. 3.3. IfVk resulting from the ZC algorithm consists of n points, which implies that CH(Vk)is a facet in A and R(r) passes through its interior, then we can switch back to theIE procedure; otherwise, we continue the ZC-based iteration.


AA( )

B

n

0

o

pA

B( )o

pB

pA

pB

n

z

AB

(a)

A

B

A( )o

pA

B( )onpB

AB

n

zpA

pB

0

(b)

Fig. 6 Illustration of reducing the growth distance between two (a) separated or (b) penetratedconvex sets A and B to a ray-shooting problem in 2-D space.

4 Application to Collision Detection

Determining the status (i.e., separation, contact, or penetration) between two objectsis a fundamental problem in robotics and other fields, such as computer animationand haptics. To do this, a natural way is to compute the minimum Euclidean distancebetween them [3, 5], but the distance computation for penetrated objects is difficultdue to the existence of many local minima. To overcome this trouble and have a uni-fied distance measure for both separated and penetrated objects, the growth distancewas proposed [10]. So far, however, there is no efficient algorithm to compute it,which significantly impeded its application. In this section, we show that the com-putation of growth distance can be reduced to a ray-shooting problem and presentnumerical results obtain using the aforementioned algorithms.

4.1 Computation of the Growth Distance

Assume that A is a compact convex set with nonempty interior in Rn, which rep-resents an object and can be specified by triangles meshes or parametric functions,as long as its support function and mapping can be computed. The object A in theglobal coordinate frame can be expressed as

A = pA +RA(A0) (7)

where pA ∈ Rn is an interior point of A indicating its position with respect to theglobal coordinate frame and RA ∈ SO(n) denotes the orientation of A, and A0 is thedescription of the object in the local coordinate frame attached at pA.

As depicted in Fig. 6, the growth model of A is defined by

A(σ), pA +σRA(A0) (8)

where σ ≥ 0. Let B be another object and B(σ) its growth model defined in the sameway as A(σ). The growth function of the object pair (A,B) is defined by [10]


g(A,B), σ∗ = minA(σ)∩B(σ )= /0, σ≥0

σ . (9)

The growth function (9) computes the minimum scale factor σ∗ such that A(σ∗)and B(σ∗) are not strictly separated from each other, which implies that A(σ∗) justcontacts B(σ∗), as shown in Fig. 6. Then, we can deduce that A and B separate ifσ∗ > 1, A and B contact if σ∗ = 1, and A and B penetrate if σ∗ < 1. Since A(σ)∩B(σ) = /0 is equivalent to 0 ∈ A(σ)−B(σ), from (8) we can rewrite (9) as

g(A,B) = min1σ (pB−pA)∈∆AB, σ≥0

σ (10)

where ∆AB =RA(A0)−RB(B0). Equation (10) implies that the calculation of g(A,B)is a ray-shooting problem between ∆AB and R(pB −pA), as depicted in Fig. 6. It canbe deduced that g(A,B) = ∥pB −pA∥/∥z∆AB(pB −pA)∥. Furthermore, from the setZ∆AB(pB −pA) and the normal n of the supporting hyperplane of ∆AB at z∆AB(pB −pA), we can derive the contact point between A(σ∗) and B(σ∗) and the normal atthe contact. Due to the page limit, we omit this derivation here.

4.2 Numerical Examples

We implement the aforementioned ray-shooting algorithms in MATLAB on a laptopwith an Intel Core i7 2.67GHz CPU and 3GB RAM and apply them to computingg(A,B) between an ellipsoid and a truncated cone, as depicted in Fig. 7. The surfaceof an ellipsoid or truncated cone is specified by parametric functions, but their sizesand relative positions and orientations are randomly generated. First, we set the twoobjects to be in contact with each other (Fig. 7a) and shrink (Fig. 7b) or enlarge(Fig. 7c) them by 2 times such that they become separated or penetrated. Then, thetrue values of g(A,B) are 1, 2, and 0.5 in the three cases (contact, separate, and pen-etrate), respectively. We generate 3000 such pairs of ellipsoids and truncated conesfor each case and report the average absolute error between the computed g(A,B)and the true values and the average CPU running time of each algorithm, as exhib-ited in Tables 1 and 2, respectively. As a comparison with the existing approach, wealso write g(A,B) as a convex programming problem as in [10] and solve it using theactive-set algorithm provided by the Optimization Toolbox of MATLAB with defaultsettings. Fig. 8a shows the average error in g(A,B) computed by the ray-shooting al-gorithms versus their termination tolerance for the three cases. Finally, we randomlygenerate 100 ellipsoids and truncated cones and compute g(A,B) between any twoof them. Then, the average running time of each algorithm is listed in the last rowof Table 2. Fig. 8b plots the running time versus the termination tolerance. Table 2also displays the number of iterations of each algorithm in the above tests.

From the numerical results, we can see that the ray-shooting algorithms are asaccurate as the active-set algorithm in MATLAB and their running times are one or-der of magnitude shorter. It is no surprise that the result accuracy can be improved


(a) (b) (c)

Fig. 7 Growth distance computation between an ellipsoid and a truncated cone. (a) An ellipsoidcontacting the side surface (upper), one edge (middle), or one base (lower) of a truncated cone.Then g(A,B) = 1. (b),(c) An ellipsoid and a truncated cone obtained by shrinking or enlargingthose in (a) by 2 times about their centroids. Then g(A,B) = 2 or g(A,B) = 0.5.

2 3 4 5 6 7 82

3

4

5

6

7

8

9

10

−log10

ε

−lo

g 10 o

f err

or

GJK−basedIEZC−basedBi−GJKZC−IE

(a)

2 3 4 5 6 7 80

1

2

3

4

5

6

−log10

ε

CP

U ti

me

(ms)


(b)

Fig. 8 Absolute error and CPU time versus the termination tolerance ε (εGJK = εZC = ε). (a)Absolute error in the computed g(A,B). (b) CPU running time of each ray-shooting algorithm.

by simply reducing the termination tolerance. It should be noted that the computa-tion costs for one iteration of these algorithms are different. Although the number ofiterations of the IE or ZC-IE procedure is bigger than or close to those of the otheralgorithms, they are still faster because the computation in each of their iteration ismuch simpler. Moreover, the running time of the IE or ZC-IE procedure increasesmuch slower as the termination tolerance decreases.


Table 1 Absolute error of each algorithm from the ground truth g(A,B). (εGJK = εZC = ε = 10−5)

GJK-based IE ZC-based Bi-GJK ZC-IE Active-Setcontact 9.62×10−7 2.91×10−6 1.24×10−6 1.68×10−7 9.10×10−7 5.14×10−7

separate 3.92×10−6 5.92×10−6 5.10×10−6 4.60×10−6 3.52×10−6 5.42×10−6

penetrate 2.42×10−7 1.51×10−6 4.82×10−7 7.04×10−7 2.18×10−7 4.85×10−7

Table 2 Average CPU running time (in milliseconds) and number of iterations (rounded up to thenext highest integer in parentheses) of each algorithm to compute g(A,B). (εGJK = εZC = ε = 10−5)

GJK-based IE ZC-based Bi-GJK ZC-IE Active-Setcontact 2.05 (2) 1.48 (15) 2.51 (20) 1.95 (5) 1.16 (17) 12.77 (9)separate 1.85 (2) 1.43 (15) 2.36 (19) 1.60 (5) 1.08 (16) 13.23 (10)penetrate 2.23 (2) 1.44 (15) 2.61 (20) 2.14 (5) 1.17 (18) 13.23 (10)random 2.08 (2) 1.28 (17) 2.01 (21) 2.02 (6) 0.97 (18) 20.05 (16)

5 Application to Contact Force Optimization

5.1 Problem Description

Consider a robot system making m contacts with the environment, such as a multi-fingered robot hand grasping an object or a legged robot standing on the ground. Tomaintain the whole system in equilibrium, the resultant wrench wres from all contactforces must counterbalance the external wrench wext (sum of the other wrenches)[9], which can be formulated with respect to the global coordinate frame as

wres =m

∑i=1

Gifi =−wext (11)

where Gi =[ ni oi ti 0

pi×ni pi×oi pi×ti ni

]∈ R6×4, pi is the position of contact i, and ni, oi

and ti are the unit normal and tangent vectors at contact i in the global coordinateframe and satisfy ni = oi × ti. The contact force fi has four components, i.e., threepure force components fi1, fi2, fi3 along ni, oi, ti, respectively, and a spin momentfi4 about ni. To maintain a stable contact, fi must stay within the friction cone [9]

Fi ,{

fi ∈ R4 | fi1 ≥ 0,

√f 2i2 + f 2

i3

µ2i

+f 2i4

µ2si≤ fi1

}(12)

where µi and µsi are the tangential and torsional friction coefficients, respectively.A major problem in the research of multi-contact robotic systems is to determine

whether there exist feasible contact forces fi ∈ Fi, i = 1,2, . . . ,m to resist a givenexternal wrench as (11) and compute the minimum contact forces if so. The overallcontact force magnitude is often measured by

σL1 ,m

∑i=1

fi1 or σL∞ , maxi=1,2,...,m

fi1. (13)


(a) (b)

Fig. 9 Examples for contact force optimization. (a) Four contacts grasping a sphere. (b) Fourcontacts grasping an ingot (also used in [16, 18]).

As described in [6, 16, 18], this problem can be reduced to a ray-shooting problembetween the ray R(−wext) and the set

WL1 , CH( m∪

i=1

Wi

)or WL∞ , CH

( m⊕i=1

Wi

)(14)

where Wi = Gi(Ui) and Ui ,{

fi ∈R4 | fi1 = 1, ( f 2i2+ f 2

i3)/µ2i + f 2

i4/µ2si = 1

}. It can

be derived that the minimum values of σL1 and σL∞ equal ∥wext∥/∥zWL1(−wext)∥

and ∥wext∥/∥zWL∞ (−wext)∥, respectively. Also, the corresponding contact forces canbe computed through the computation of ZWL1

(−wext) or ZWL∞ (−wext). The detailedderivation can be found in [6, 16, 18] and is omitted here due to the page limit.

As an extension to the previous work [6, 16, 18], we would like to point out herethat the ray-shooting algorithms also work in the case where the overall contactforce magnitude is measured and needs to be minimized as

σL1 ,m

∑i=1

∥fi∥ or σL∞ , maxi=1,2,...,m

∥fi∥ (15)

where ∥fi∥ ,√

f 2i1 + f 2

i2 + f 2i3 +(µ2

i /µ2si) f 2

i4 is the magnitude of fi rather than its

normal component fi1 used in (13). To do this, we only need to redefine Ui ,{

fi ∈Fi | f 2

i1+ f 2i2+ f 2

i3+(µ2i /µ2

si) f 2i4 = 1

}. The derivation and the test of the ray-shooting

algorithms to compute minimum contact forces in terms of a different force magni-tude measure will be included in a complete version of this paper.

5.2 Numerical Examples

We first verify the accuracy of the ray-shooting algorithms applied to the contactforce optimization with the example shown in Fig. 9a, where four contacts are lo-cated symmetrically on a sphere and have the same elevation angle α . Then, itcan be derived that the minimum normal contact force for holding the sphere is


Table 3 Relative error of each algorithm to minimize σL1 .

λ GJK-based IE ZC-based Bi-GJK ZC-IE Active-Set10−1 6.86×10−16 4.93×10−6 0 0 0 3.63×10−9

10−2 1.35×10−14 1.98×10−6 0 1.83×10−16 0 1.65×10−6

10−3 1.38×10−13 5.38×10−6 6.88×10−14 6.91×10−14 6.88×10−14 2.88×10−8

10−4 0 2.84×10−6 6.89×10−13 6.89×10−13 6.89×10−13 3.92×10−3

10−5 6.89×10−12 infeasible infeasible infeasible infeasible 1.09×10−1

10−6 infeasible infeasible infeasible infeasible infeasible infeasible

G/4(µ cosα − sinα) for each contact and no feasible contact forces exist to do soif α ≥ tan−1 µ , where G is the gravity of the sphere and µ is the tangential fric-tion coefficient. Thus, the minimum values of σL1 and σL∞ are G/(µ cosα − sinα)and G/4(µ cosα − sinα), respectively. Here, we take G = 10 N, µ = 0.2, andα = (1 − λ ) tan−1 µ for λ = 10−1,10−2, . . . ,10−6, respectively. The terminationtolerance is 10−5 for each ray-shooting algorithm. For comparison, we also for-mulate the contact force optimization as a convex programming problem and solveit using the active-set algorithm provided by MATLAB, for which the maximumnumber of function evaluations and the maximum number of iterations are both setto be sufficiently big such that its result can be comparably accurate. Tables 3 and4 display the relative error in the result of each algorithm compared with the trueminimum value of σL1 or σL∞ . It is shown that, in this particular case, the results ofthe ray-shooting algorithms except the IE procedure are highly accurate. Actually,they compute the minimum contact forces by evaluating the support mapping of WL1or WL∞ only once, which can be done analytically as discussed in [15], and the errorin their results is just the round-off error. As the elevation angle α of each contactapproaches tan−1 µ , some algorithms report that the problem becomes infeasible.This is because the termination tolerance ε is too big for those cases. To relieve thisnumerical issue and obtain a more accurate result, we have to choose a smaller ε .

We also verify the efficiency of these algorithms with the grasping example usedin [16, 18], as shown in Fig. 9. We compute contact forces with minimum σL1 orσL∞ with respect to 103 random wext and report the average CPU running time andnumber of iterations of each algorithm, as exhibited in Table 5. It can be seen thatthe ray-shooting algorithms are several times faster than the active-set algorithm inMATLAB. In particular, the IE and ZC-IE procedures are more efficient than theother ray-shooting algorithms due to the much simpler computation in their itera-tions, though they need more iterations. Also, their running times rise slower alongwith the decrease of the termination tolerance, as revealed in Fig. 10. Furthermore,the ZC-IE procedure is even faster than the IE procedure because it saves the com-putation for maintaining the facet and ridge collections, which is only required onthe rare occasion that the ray passes through a face of low dimension. In our imple-mentation of algorithms, the IE and ZC-IE procedures have the same initialization.Then, the difference in their numbers of iterations shown in Table 5 implies that theoccasion happens and causes the two procedures to follow different iteration steps.


Table 4 Relative error of each algorithm to minimize σL∞ .

λ GJK-based IE ZC-based Bi-GJK ZC-IE Active-Set10−1 0 6.53×10−6 0 0 0 7.50×10−9

10−2 0 5.49×10−6 0 6.96×10−15 0 1.18×10−8

10−3 1.38×10−13 5.19×10−6 1.38×10−13 1.38×10−13 1.38×10−13 5.63×10−6

10−4 0 5.39×10−6 0 0 0 9.65×10−5



Table 5 Average CPU running time (in milliseconds) and number of iterations (rounded up tothe next highest integer in parentheses) of each algorithm for contact force optimization.

GJK-based IE ZC-based Bi-GJK ZC-IE Active-Setminimize σL1 13.80 (4) 6.50 (33) 10.22 (26) 12.11 (8) 4.57 (30) 38.94 (24)minimize σL∞ 22.23 (4) 10.55 (54) 18.27 (47) 19.25 (10) 7.93 (53) 47.36 (31)

1 2 3 4 5 6 70

10

20

30

40

50

60

70

−log10

ε

CP

U ti

me

(ms)


(a)

1 2 3 4 5 6 70

10

20

30

40

50

60

70

−log10

ε

CP

U ti

me

(ms)


(b)

Fig. 10 CPU running time versus the termination tolerance ε (εGJK = εZC = ε) of each ray-shooting algorithm to minimize (a) σL1 or (b) σL∞ .

6 Conclusions and Future Work

In this paper, we discussed general ray-shooting algorithms and their applicationsin robotics. We first described three individual procedures and their hybrid uses tocompute the ray-shooting problem. They do not use any existing optimization tech-niques and their implementation is very straightforward. Then, their performancequalities, including the computational accuracy and efficiency, have been verifiedand compared with each other in the application to growth distance computationand contact force optimization. Numerical examples show that the ray-shooting al-gorithms provide better ways to solve these fundamental problems in robotics thansome existing methods. Particularly, the ZC-IE procedure, which is newly proposedin this paper, is notably faster than the other ray-shooting algorithms and able tocompute results at the same level of accuracy.

Because of the page limit, we could not go into the implementation details ofthese algorithms and leave it for future publications. As future work, we also wouldlike to apply the algorithms to a dynamic environment, such as collision detectionfor moving objects or contact force optimization with respect to a time-varyingexternal wrench. Then, by taking advantage of time coherence, they may achieveconstant time complexity. We will conduct more experiments on the algorithms,


especially in dynamic environments, and report their results in a complete versionof this paper. By doing this, we expect to give a comprehensive evaluation of theirperformance and figure out how to choose an algorithm for a specific problem. Inaddition, we are exploring other applications of these ray-shooting algorithms.

References

1. P. K. Agarwal and J. Matousek. Ray shooting and parametric search. In Proceedings of the24th ACM Symposium on Theory of Computing, pages 517–526, 1992.

2. D. Ding, Y.-H. Liu, Y. Wang, and S. G. Wang. Automatic selection of fixturing surfaces andfixturing points for polyhedral workpieces. IEEE Transactions on Robotics and Automation,17(6):833–841, 2001.

3. E. G. Gilbert, D. W. Johnson, and S. S. Keerthi. A fast procedure for computing the distancebetween complex objects in three-dimensional space. IEEE Transactions on Robotics andAutomation, 4(2):193–203, 1988.

4. S. R. Lay. Convex Sets and their Applications. John Wiley & Sons, New York, NY, USA,1982.

5. M. Lin and J. Canny. A fast algorithm for incremental distance calculation. In Proceedingsof the IEEE International Conference on Robotics and Automation, pages 1008–1014, Sacra-mento, CA, 1991.

6. Y.-H. Liu. Qualitative test and force optimization of 3-D frictional form-closure grasps usinglinear programming. IEEE Transactions on Robotics and Automation, 15(1):163–173, 1999.

7. Y.-H. Liu, M.-L. Lam, and D. Ding. A complete and efficient algorithm for searching 3-Dform-closure grasps in the discrete domain. IEEE Transactions on Robotics, 20(5):805–816,2004.

8. J. Matousek and O. Schwarzkopf. On ray shooting in convex polytopes. Discrete Computa-tional Geometry, 10(1):215–232, 1993.

9. R. M. Murray, Z. X. Li, and S. S. Sastry. A Mathematical Introduction to Robotic Manipula-tion. CRC Press, Boca Raton, FL, USA, 1994.

10. C. J. Ong and E. G. Gilbert. Growth distance: New measures for object separation and pene-tration. IEEE Transactions on Robotics and Automation, 12(6):888–903, 1996.

11. C. J. Ong and E. G. Gilbert. Robot path planning with penetration growth distance. Jounralof Robotic Systems, 15(2):57–74, 1998.

12. W. Rudin. Principles of Mathematical Analysis. McGraw-Hill, New York, NY, USA, thirdedition, 1976.

13. L. Szirmay-Kalos, V. Havran, B. Balazs, and L. Szecsi. On the efficiency of ray-shootingacceleration schemes. In Proceedings of the Spring conference on Computer Graphics, pages97–106, Budmerice, Slovakia, 2002.

14. G. van den Bergen. Ray casting against general convex objects with application to continuouscollision detection. http://www.dtecta.com, 2004.

15. Y. Zheng and C.-M. Chew. Distance between a point and a convex cone in n-dimensionalspace: computation and applications. IEEE Transactions on Robotics, 25(6):1397–1412, 2009.

16. Y. Zheng and C.-M. Chew. A numerical solution to the ray-shooting problem and its applica-tions in robotic grasping. In Proceedings of the IEEE International Conference on Roboticsand Automation, pages 2080–2085, Kobe, Japan, May 2009.

17. Y. Zheng, C.-M. Chew, and A. H. Adiwahono. A GJK-based approach to contact force fea-sibility and distribution of multi-contact robots. Robotics and Autonomous Systems, 59(3-4):194–207, 2011.

18. Y. Zheng, M. C. Lin, and D. Manocha. A fast n-dimensional ray-shooting algorithm forgrasping force optimization. In Proceedings of the IEEE International Conference on Roboticsand Automation, pages 1300–1305, Anchorage, Alaska, May 2010.

Documents

Ray-Shooting Algorithms for Robotics · Ray-Shooting Algorithms for Robotics 5 (a) (b) Fig. 3 Illustration of the GJK-based ray-shooting algorithm in 2-D space. (a) Iteration. b0