Robotic manipulation project

Robotic manipulation project Bin Nguyen December 5, 2006 Abstract Tis is te draft report for Robotic Manipulation s class project. Te cosen project aims to understand and implement Kevin Egan s non-convex constraint[5] as an extended Linear Complementarity problem[6]. Tis draft contains all te background teories along wit analysis needed to complete te project and te implementation progress. 1 Introduction Te main objective of tis project is to study Kevin Egan s metod[5] and apply it to properly model general 2D polyedral sapes using LCPs. Due to te limited time, I will just focus on simple test scene wic contains a moving spere and polyedral obstacles. Tat will simplify my collision detection. Also, in term of teories and setting LCPs, tis simple scene is similar to general polyedrons - polyedrons cases. Also, I will not consider friction because it will only complicate implementation. Te metod extends well to frictional cases. 1.1 Stewart-Trinkle LCP model Stewart-Trinkle [7] non-penetration constraints maintain a list of active pairs of vertex, edge and try to keep future position of te vertices from penetrating corresponding edges in contact normal direction. Tis is te same metod used in oter LCP models wic can be studied in [7],[8], [1],[2].Tese models linearized contact constraints suc tat eac contact constraint defines a alf-space in configuration space. Tus, tey can only model locally convex configuration space. Tis implicit assumption leads to artifacts in simulation. Note tat te artifacts only visible in in simulation wen two vertices are close enoug. So, te problems are bearable in some applications. A simple example tat exibits tis artifact is sowed in Figure 1 wen a point mass approaces a vertex in 2D. 1

Configuration space Point mass Obstacle Simulated position Correct position Obstacle Obstacle Figure 1: Stewart-Trinkle non-penetration constraints in non-convex configuration space. Te non-penetration constraints in tis case are simply te point mass cannot penetrate bot edges at te same time. Te point mass cannot penetrate edge 1: 0 W1nν T l+1 + ψl 1 Te point mass cannot penetrate edge 2: 1n 0 (1) In wic: 0 W T 2nν l+1 + ψl 2 2n 0 (2) ν l+1 generalized velocity at te end of time step [t l, t l+1 ]. time step size. W in in normal contact wrenc corresponds to edge it. unknown vector represents magnitude of impulse at contact it. ψ l i current distance between two contact features of contact it. If we define: W T inν l+1 + f l i ρl+1 (3) Ten ρ i is te projected distance between corresponding vertex and edge along current normal of contact it at time t l+1. 2

We can see tat Stewart-Trinkle metod limits te simulated possible position of te point mass in te area of ρ 1 0 ρ 2 0 wic is more restricted tan te pysically correct one. We can address tis problem by using Kevin Egan s non-convex non-penetration constraint. 1.2 Kevin Egan s model for a locally non-convex corner Kevin Egan s non-convex non-penetration constraints can model non-convex configuration space wit te price of more linear complementarity constraints. In tis 2D simple example, te possible position of te point mass is sowed in figure 2. Configuration space Point mass Correct position = Simulated position Obstacle Figure 2: Kevin Egan non-penetration constraints in non-convex configuration space. Kevin Egan s non-convex model as te ability to or te two conditions instead of and.so te possible future position of te point mass are ρ 1 0 ρ 2 0. I will spend te next section on ow to use linear complementarity problems to model or operator. 1.3 Extending Kevin Egan s model for group of vertices Figure 3 and 4 sow an arbitrary part of a polyedral sape. In order to model suc sape using Kevin Egan s metod, we will break te edges into groups follow te rules: 1. If a group contains edges {e i1, e i2,..., e ik } ten te configuration space corresponds to tat group will be modeled by te set of points tat satisfy ρ i1 0 ρ i2 0... ρ ik 0. In oter words, a group tat contains more tan one edge sould form a locally nonconvex configuration space or convex obstacle space. 3

2. Tere sould be no edge belong to two groups. Actually, tis rule is just for efficiency purpose. Te result sould still be te same if we enforce te same constraint more tan once. 3. Te final configuration space will be modeled by te set of points tat satisfy all groups s condition. So te configuration space will be modeled as te area tat satisfy: (ρ 11 0...ρ 1k1 0) (ρ 21 0...ρ 2k2 0)...(ρ n1 0...ρ nkp 0). 4. Usually te less number of groups, te better performance. We will see tis in later section wen we discuss about ow to model tese conditions. One ting wort mentioning is tat tese rules won t guarantee tat we will model an absolutely accurate configuration space. Te accuracy depends on ow we break te edges into groups. But tis metod provide us a muc more powerful tool to model locally nonconvex space. Also, tere are maybe oter sets of rules to partition te set of edges but tey are beyond te scope of tis project. Figure 3: A accurate way to group edges. Figure 4: An inaccurate way to group edges. 4

In figure 4, point P is incorrectly classified as valid because it satisfies condition ρ 3 0. Tis partition violates rule number 3. If tere is an algoritm to partition any polyedral sapes follow tese rules ten tis set of rules are complete. I leave tat algoritm to future work. Remark I don t ave a proof tat we will always be able to accurately model any 2D polyedral obstacle following te rules but I aven t found any counter example. 2 Modeling locally non-convex space using LCP Tis section discusses te metod to model te or operator using LCPs and extend it to model a group of non-convex constraints described in previous section. Kevin Egan reported in [5] two formulations: correct max and summation wit small error. But tese two formulations are superseded by a new diff formulation wic was developed later in a note by Kevin and Jong-Si Pang[3]. Te new formulation as two forms: one simpler but less accurate and one complicated and accurate. For eac form, I will first present metod to model a simple group wit only 2 sides and ten extend it to group wit n sides. 2.1 Less accurate form Te reason I call tis form is less accurate because altoug it properly models te configuration space described by te group it allows normal impulses exist at any edges in te group even if te distances corresponding to tose edges are not zero. 2.1.1 Simple case: group wit two sides If we name te group as {e 1, e 2 } ten te configuration space we want to model is te set of points tat satisfy condition: is: ρ 1 0 ρ 2 0 max{ρ 1, ρ 2 } 0. (4) So if we call normal impulse at edge 1 as p 1n ten tis form s non-penetration constraint 0 p 1n max{ρ 1, ρ 2 } 0 (5) 0 p 1n max{ρ 1 ρ 2 } + ρ 2 0 (6) We will use LCP to model te function max{a, 0} using following observation: Proposition 2.1 c = max{a, 0} if and only if 0 c a c 0. Proof We prove tis proposition by cecking all possible cases of a s value and make sure in all cases, te results of te two are te same. 5

Case 1: a < 0: max{a, 0} = 0 and c a > 0 c = 0. Case 2: a = 0: max{a, 0} = 0 and c a = c c = 0. Case 3: a > 0: max{a, 0} = a and because c a 0 c > 0 c = a So te penetration constraint can be written in LCPs form as: 0 c 1 ρ 1 + ρ 2 c 1 0 0 c 1 + ρ 2 1n 0 Similarly, we can write constraint for edge number 2. Ten te final non-penetration constraints for te group are: 0 c 1 ρ 1 + ρ 2 c 1 0 (7) 0 c 1 + ρ 2 1n 0 (8) 0 c 1 + ρ 2 2n 0 (9) So te cost for tis formulation is one more LCP condition. 2.1.2 Extended case: group wit n sides In tis case, te group contains n edges so te configuration space is te set of points tat satisfy: ρ 1 0... ρ n 0 max{ρ 1,..., ρ n } 0. (10) Ten te penetration constraint for edge 1 is: 0 p 1n max{ρ 1,, ρ n } 0 0 p 1n max{ρ 1, max{ρ 2,, max{ρ n 1, ρ n } }} 0 We can use n 1 max(a, b) to get max{ρ 1,, ρ n } so te penetration constraints for te group written in LCP form are: 6

0 c 2 ρ 2 + ρ 1 c 2 0 0 c 3 ρ 3 + c 2 + ρ 1 c 3 0 0 c k ρ k + c k 1 + + c 2 + ρ 1 c k 0 0 c n ρ n + c n 1 + + c 2 + ρ 1 c n 0 0 c 2 + c 3 + + c n + ρ 1 1n 0 0 c 2 + c 3 + + c n + ρ 1 2n 0 0 c 2 + c 3 + + c n + ρ 1 nn 0 Te total number of linear complementarity conditions is 2n 1 so te price of modeling tis non-convex group is n 1 conditions. 2.2 Accurate metod Te previous formulation could properly model non-convex configuration space but it assumes tat normal impulses can exist in all edges in te group. But it doesn t mean tat we will always get normal impulse from every edges using previous equation, te LCP solver may properly coose te rigt edge. I present a more complicated formulation described in Kevin Egan and Jong-Si Pang s note[3] tat force normal impulse can only exist at te rigt edges wen te contacts maintained. Again, I will go troug simple 2 edges group first, ten extend it to many edges group. 2.2.1 Simple case: group wit two sides Te main difference is we now force p 1n > 0 only wen ρ i = 0. So te normal impulse at edge 1: p 1n > 0 (max{ρ 1, ρ 2 } = 0) (ρ 1 = 0) (11) We model tis equation using LCPs using following propositions: Proposition 2.2 (max{ρ 1, ρ 2 } = 0) (ρ 1 = 0) (max{ρ 1, ρ 2 } = 0) (max{ρ 1, ρ 2 } + min{ρ 1, 0} = 0). Proof We need to prove tat wen max{ρ 1, ρ 2 } = 0 ten min{ρ 1, 0} = 0 ρ 1 = 0. ρ 1 = 0 min{ρ 1, 0} = min{0, 0} = 0 7

min{ρ 1, 0} = 0 max{ρ 1, ρ 2 } = 0 ρ 1 = 0 We can also model min{0, 0} using LCP: Proposition 2.3 if min{a, 0} = b ten 0 b + a b 0. Proof Case a < 0: min{a, 0} = a and b + a 0 b > 0 a + b = 0 b = a. Case a = 0: min{a, 0} = 0 and 0 b b 0 b = 0 Case a > 0: min{a, 0} = 0 and b 0 a + b > 0 b = 0. So te final accurate formulation for non-penetration constraint at edge 1 is: 0 c 1 ρ 1 + ρ 2 c 1 0 0 g 1 + ρ 1 g 1 0 0 c 1 + ρ 2 + g 1 1n 0 0 c 1 + ρ 2 p 0 And for edge 2, we need to add two LCPs: 0 g 2 + ρ 2 g 2 0 0 c 1 + ρ 2 + g 2 2n 0 So, te cost of accuracy in tis formulation is four linear complementarity conditions more tan te non-accurate one. In wic, two conditions are used for moodeling non-convexity, and two conditions for selecting te rigt edges were collisions appen. 2.2.2 Extended case: group wit n sides In tis case, we only need to add n LCPs to force impulse force only exist in constraints correspond to edges tat as distance zero. So te final LCPs formulation is: 8

0 c 2 ρ 2 + ρ 1 c 2 0 0 c 3 ρ 3 + c 2 + ρ 1 c 3 0 0 c k ρ k + c k 1 + + c 2 + ρ 1 c k 0 0 c n ρ n + c n 1 + + c 2 + ρ 1 c n 0 0 g 1 + ρ 1 g 1 0 0 g 2 + ρ 2 g 2 0 0 g n + ρ n g n 0 0 g 1 + c 2 + c 3 + + c n + ρ 1 1n 0 0 g 2 + c 2 + c 3 + + c n + ρ 1 2n 0 0 g n + c 2 + c 3 + + c n + ρ 1 nn 0 0 c 2 + c 3 + + c n + ρ 1 p 0 Te number of linear complementarity conditions to model a non-convex group of n edges using tis accurate formulation is 3n. In wic: n to model non-convex configuration space n to coose proper edges of contact n to model non-penetration constraints So in general, te price of accuracy in tis new formulation is 2n more linear complementarity conditions. 3 Implementation 3.1 Putting non-convex constraints in Stewart-Trinkle framework We know tat: ρ l+1 n = Ψl n + W n T ν l+1 + Ψl n So we can express ρ i n l+1 as function of ν l+1 to put tem in Stewart-Trinkle framework [8]. Below is te complete MLCP formulation I derived using Kevin Egan s accurate formulation 9

for a non-convex group wit n sides. It s easy to extend to many groups by just duplicating complementarity conditions accordingly. 0 M 0 0 0 0 W 1n W nn 0 ν l+1 ζ 2 (W1n T W2n) T 1 c 2 ζ 3 (W1n T W3n) T 1 1 c 3 ζ n (W1n T Wnn) T 1 1 c n γ 1 W1n T 0 0 1 g 1 γ 2 = W2n T 0 0 0 1 g 2 γ n W T ρ l+1 nn 0 0 0 1 g n 1 W T ρ l+1 1n 1 1 2 W2n T 1n 1 1 0 1 2n ρ l+1 n Wnn T 1 1 0 1 W1n T nn 1 1 ρ l+1 + Ψ l 1n Ψ l 1n Ψ l 1n Mν l + p l ext + Ψl 1n + Ψl 1n + Ψl 1n Ψ l 1n Ψ l 2n Ψ l nn Ψ l 1n Ψ l 2n Ψ l nn Ψ l 1n Ψl 2n Ψl 3n Ψl nn + Ψl 1n + Ψl 2n + Ψl nn + Ψl 1n + Ψl 2n + Ψl nn + Ψl 1n + Ψl 2n + Ψl 3n + Ψl nn ; ζ 2 c 2 ζ 3 c 3 ζ n c n γ 1 g 1 γ 2 g 2 γ n g n ρ l+1 1 ρ l+1 1n 2 2n ρ l+1 n ρ l+1 nn (12) Te big matrix we usually called B for one n sides group is 3n + 2 3n + 2. 3.2 Project s implementation I ave finised te coding part to test tese new formulations. Te demo program contains tree time-steppers: Original Stewart-Trinkle time-stepper 10

Kevin Egan s simple non-convex formulation Kevin Egan s accurate non-convex formulation Below is a screensot of te demo running wit simple non-convex formulation: Here are te list of some features built in tis demo: Read geometry from external file Allow simple user interaction Work wit moving particle and static polyedral sapes In sort, it can be used to test Kevin Egan s formulations. 4 Some interesting results In tis section, I will provide some interesting results from implementing Kevin Egan s formulation. 4.1 Simple formulation Te demo program sows tat te metod works as expected. We really get a non-convex configuration space as te particle could travel pass te virtual edge smootly. But once in contact wit te edges, te particle will automatically move along te edge as we allow normal impulses at all edges. Tis effect is sowed below: 11

Te particle can travel troug virtual edge to get in contact wit te edges. But once in contact, it will move along te edge. Te direction of te move depends on te edges s normal. It sows tat we can overcome te virtual edge s artifact caused by convex assumption of original Stewart-Trinkle time stepper but it leads to anoter problem. Actually, depend on applications, tis problem can be treated in some ways. One way of wic is cecking n for in contact event. If it s in contact ten switc to original Stewart-Trinkle time stepper. 4.2 Accurate formulation After a long time trying to implement tis more complicated formulation, I found out tat tere is one linear complementarity condition tat leads to singularity in our big matrix. And tis singularity make my LCP s solver, PATH [4], fails (crased) after some time steps. 12

Tat condition is: 0 c 2 + c 3 + + c n + ρ 1 p 0 In wic, p is just a dummy variable and no oter conditions need p. Tat leads to a full column of 0 in te big matrix. I ave tried to introduce an small ɛ amount of p in te left and side but it makes te simulation unstable. Maybe tere is oter way of dealing wit it but in a sort time, I could not tink of any. Below is te screen-sot tat sows te big matrix in simulation: 4.3 Can Kevin Egan s metod model precisely all planar obstacle? Rigt now I don t ave an absolute answer for polyedron vs polyedral case but te answer for te case wit very simple implicit object: circle vs polyedron is NO. Below is te case were Kevin Egan s metod doesn t work wit circle: Tis is completely valid configuration but Kevin Egan s metod will report it as invalid because bot distances are negative. 5 Future works Tere are some areas we can explore in te future: 13

Using topology information of a polyedral sape in convex formulation. Advance into 3D. Work wit implicit surfaces by discretizing te surfaces on te fly and model it wit convex/non-convex obstacle. Find an algoritm to partition arbitrary polyedral sape into groups. Fix te accurate formulation References [1] M. Anitescu and F.A. Potra. A time-stepping metod for stiff multibody dynamics wit contact and friction. International Journal for Numerical Metods in Engineering, 55: 753 784, 2002. [2] D. Baraff. Issues in computing contact forces for non-penetrating rigid bodies. Algoritmica, pages 292 352, October 1993. [3] Kevin Egan. More tougts on nonconvex constraints. 2004. [4] Micael C. Ferris and Todd S. Munson. Pat cp solver. URL ttp://www.cs.wisc. edu/cpnet/cpnetsoftware/. 2002. [5] S. Berard K.T. Egan and J.C. Trinkle. Modeling nonconvex constraints using linear complementarity. Tecnical Report 03-13, 2004. [6] Katta G. Murty. Linear Complementarity, Linear and Nonlinear Programming Internet Edition. Helderman-Verlag, 1988. URL ttp://ioe.engin.umic.edu/people/fac/ books/murty/linear_complementarity_webbook/. [7] D.E. Stewart and J.C. Trinkle. An implicit time-stepping sceme for rigid body dynamics wit inelastic collisions and coulomb friction. Int l Jrnl. of Numerical Metods in Engineering, pages 2673 2691, 1996. [8] D.E. Stewart and J.C. Trinkle. An implicit time-stepping sceme for rigid body dynamics wit inelastic collisions and coulomb friction. International Journal of Numerical Metods in Engineering, 39:2673 2691, 1996. 14