Computing Feasible Contact Forces

Here, I introduce an efficient algorithm to solve problem (4.23) and compute the minimum distance betweenwandV (Zheng and Chew, 2009). It generates a sequence of simplicial conesCO(Vk)inV such that their minimum distances to the pointwconverge to the minimum distance betweenwandV, as illustrated in Fig. 4.4. Ifw ∈/ V, then the pointwˆ inV closest towin terms of the Euclidean distance will be computed as well, and it represents the closest wrench to the required onewthat can be generated with feasible contact forces for tracking the reference motion.

Fig. 4.4 illustrates the iteration steps of the algorithm. Suppose that I have a linear- ly independent subset Vk of W such that w ∈/ CO(Vk) but w ∈/ CO(Vk). Let dk ,

minv∈CO(Vk)kw −vk be the minimum distance between w and CO(Vk) as defined by (4.23),vk the point inCO(Vk)such thatdk = kw−vkk, andVˆk a minimal subset ofVk such thatvk can be written as a positive combination ofVˆk. Since w ∈/ CO(Vk), I have

passing throughvk. Then, it turns out thatHksupportsCO(Vk)such thatwlies on the side ofHkthatnkpoints to andCO(Vk)falls on the other side.

The algorithm iterates by

Vk+1 = ˆVk∪ {sW(nk)} (4.37)

wheresW(nk)is the support mapping of W alongnk as defined by (4.31). Since vk ∈

CO(Vk) ⊂ CO(W), the hyperplane Hk cuts W such thathW(nk) ≥ 0always holds. If hW(nk)>0, thensW(nk)is on the same side ofHkaswand different side fromCO(Vk). As a consequence, it can be proved that dk+1 , minv∈CO(Vk+1)kw −vk, namely the

minimum distance betweenwandCO(Vk+1), is strictly smaller thandk. Therefore,dkis strictly decreasing along with the iteration and guaranteed to converge to the minimum distance betweenwandV, which is the greatest lower bound ondkand can be obtained when hW(nk) = 0. Thereby, the criterion for stopping the iteration is taken to behW(nk)< . Along with the convergence ofdk, the pointvk converges to the point inV closest tow, which I denote bywˆ.

In addition to the minimum distance betweenw and V, the algorithm returns other important values so that I can compute the corresponding feasible contact forces to generate

wifw∈V orwˆ ifw∈/ V. Note thatVkˆ is a linearly independent subset ofW such that

vkcan be written as its positive combination at every iteration. Asvk converges towˆ,Vˆk finally gives a linearly independent subset ofW, denoted byWˆ, such thatwˆ can be written as its positive combination

ˆ w = n X j=1 cjwj withcj >0 for∀j. (4.38)

From (4.37) it can be seen that every point inWˆ is obtained by calculating the support mappingsW ofW along a certain direction. In the case whereW is defined as (4.29a), from (4.32a) and (4.33) I obtain a contact indexij ∈[1, m]and a primitive contact forcesj ∈Uij

for eachwj ∈Wˆ such thatwj =Gijsj, which means thatwj is generated by the primitive contact forcesj at contactij. Substituting it into (4.38), I then obtain

ˆ w= n X j=1 cjGijsj withcj >0 for∀j. (4.39)

As a result, I derive the contact forcesfi ∈Fi(i= 1,2, . . . , m) for generatingwˆ as

fi = n X j=1 ij=i cjsj. (4.40)

Sincesj is a primitive contact force and satisfies the friction constraint and all coefficients cj are positive,fi computed by4.40is guaranteed to satisfy the friction constraint.

IfW is given by (4.29b) andσby (4.25b), from (4.32b) and (4.33) I have a set of contact indices{ij,1, ij,2, . . . , ij,mj}and a set of primitive contact forces{sj,1,sj,2, . . . ,sj,mj}for each wj ∈ Wˆ such that wj =

Pmj

l=1Gij,lsj,l, where ij,l ∈ [1, m] and sj,l ∈ Uij,l for l = 1,2, . . . , mj andmj ≤ m. This means thatwj is the resultant wrench from primitive contact forces atmj contacts. From (4.38), I then derive

ˆ w = n X j=1 cj mj X l=1 Gij,lsj,l withcj >0 for∀j. (4.41)

Therefore, I obtain another set of feasible contact forces to generatewˆ as

fi = n, mj X j=1, l=1 ij,l=i cjsj,l. (4.42)

Algorithm 1 Algorithm for feasible contact forces Input: wandW

Output: Feasible total contact forcef

1: V0 ←any linearly independent subset ofW,k ←0

2: v0 ←point inCO(V0)whose distance fromwis the minimum distance betweenwand

CO(V0)

3: Vˆ0 ←minimal subset ofV0 such thatv0 ∈CO( ˆV0) 4: n0 ← −v0/kv0k2

5: whilehW(nk)> do

6: Vk+1 ←Vˆk∪ {sW(nk)},k ←k+ 1

7: vk ←point inCO(Vk)whose distance fromwis the minimum distance betweenw andCO(Vk)

8: Vˆk ←minimal subset ofVksuch thatvk∈CO( ˆVk) 9: nk← −vk/kvkk2

10: end while

11: Computefiby (4.40) or (4.42) 12: return f