SAHandplusTeapot.world
Go to the documentation of this file.
1 
2 [CONSTANTS]
3 
4 % Radius of the spheres representing contact points
5 rc:=0.1
6 
7 % radius of the sphere modeling the fingertip
8 rho:=18
9 
10 % radius of the sphere, the object itself
11 RS:=70
12 
13 % qji_k is a vector expreseing the anchor point of reference frame i with respect to reference frame k of finger j.
14 % if k is not given, it means it is respect to the palm, which in our case, it is the absoulute one.
15 
16 q11x := -4.3
17 q11y := -40.165
18 q11z := 145.43
19 %q11 = q12
20 
21 q21x := -4.3
22 q21y := 0
23 q21z := 150.15
24 %q21 = q22
25 
26 q31x := -4.3
27 q31y := 40.165
28 q31z := 145.43
29 %q31 = q32
30 
31 q40x := -3
32 q40y := 27.1
33 q40z := 0
34 
35 q41x_0:= 97
36 q41y_0:= 6
37 q41z_0:=-87
38 %q41_0 = q42_0
39 
40 
41 % These anchor points can be seen as the length of the phalanges,
42 % but that is because the phalanges are aligned with this vectors.
43 q13x_2 := 67.8
44 q13y_2 := 0
45 q13z_2 := 0
46 
47 q14x_3 := 30
48 q14y_3 := 0
49 q14z_3 := 0
50 
51 
52 q23x_2 := 67.8
53 q23y_2 := 0
54 q23z_2 := 0
55 
56 q24x_3 := 30
57 q24y_3 := 0
58 q24z_3 := 0
59 
60 
61 q33x_2 := 67.8
62 q33y_2 := 0
63 q33z_2 := 0
64 
65 q34x_3 := 30
66 q34y_3 := 0
67 q34z_3 := 0
68 
69 
70 q43x_2 := 67.8
71 q43y_2 := 0
72 q43z_2 := 0
73 
74 q44x_3 := 30
75 q44y_3 := 0
76 q44z_3 := 0
77 
78 
79 % Fingertip point vectors, qj5_4, and fingertip normal vector, mj_4
80 q15x_4 := 20
81 q15y_4 := 0
82 q15z_4 := 0
83 m1x_4 := 0.14
84 m1y_4 := 0
85 m1z_4 := 0.99
86 
87 
88 q25x_4 := 20
89 q25y_4 := 0
90 q25z_4 := 0
91 m2x_4 := 0.14
92 m2y_4 := 0
93 m2z_4 := 0.99
94 
95 
96 q35x_4 := 20
97 q35y_4 := 0
98 q35z_4 := 0
99 m3x_4 := 0.14
100 m3y_4 := 0
101 m3z_4 := 0.99
102 
103 q45x_4 := 20
104 q45y_4 := 0
105 q45z_4 := 0
106 m4x_4 := 0.14
107 m4y_4 := 0
108 m4z_4 := 0.99
109 
110 
111 % Palm contact points vectors, and normal vector.
112 %point 1
113 qpx := 20.550-1*rho
114 qpy := -0.053
115 qpz := 117.348
116 mpx := 1
117 mpy := 0
118 mpz := 0
119 
120 beta_p := pi/3
121 
122 % Joint ranges
123 
124 % Two ranges for the universal joint connecting the palm and finger 1
125 phi_1_1_min:= -pi/8
126 phi_1_1_max:= pi/8
127 
128 phi_1_2_min:= 0
129 phi_1_2_max:= pi/2
130 
131 % Range for the first rotation in finger 1
132 phi_1_3_min:= 0
133 phi_1_3_max:= pi/2
134 
135 % Range for the second rotation in finger 1
136 phi_1_4_min:= 0
137 phi_1_4_max:= pi/2
138 
139 % Range for contact point in finger 1
140 beta_1:= pi/2
141 
142 % Two ranges for the universal joint connecting the palm and finger 2
143 phi_2_1_min:= -pi/8
144 phi_2_1_max:= pi/8
145 
146 phi_2_2_min:= 0
147 phi_2_2_max:= pi/2
148 
149 % Range for the first rotation in finger 2
150 phi_2_3_min:= 0
151 phi_2_3_max:= pi/2
152 
153 % Range for the second rotation in finger 2
154 phi_2_4_min:= 0
155 phi_2_4_max:= pi/2
156 
157 % Range for contact point in finger 2
158 beta_2:= pi/2
159 
160 % Two ranges for the universal joint connecting the palm and finger 3
161 phi_3_1_min:= -pi/8
162 phi_3_1_max:= pi/8
163 
164 phi_3_2_min:= 0
165 phi_3_2_max:= pi/2
166 
167 % Range for the first rotation in finger 3
168 phi_3_3_min:= 0
169 phi_3_3_max:= pi/2
170 
171 % Range for the second rotation in finger 3
172 phi_3_4_min:= 0
173 phi_3_4_max:= pi/2
174 
175 % Range for contact point in finger 3
176 beta_3:= pi/2
177 
178 % Two ranges for the universal joint connecting the palm and finger 4
179 phi_4_0_min:=0
180 phi_4_0_max:= 0
181 
182 phi_4_1_min:= 0
183 phi_4_1_max:= 0
184 
185 phi_4_2_min:= 0
186 phi_4_2_max:= 0
187 
188 % Range for the first rotation in finger 4
189 phi_4_3_min:= 0
190 phi_4_3_max:= 0
191 
192 % Range for the second rotation in finger 4
193 phi_4_4_min:= 0
194 phi_4_4_max:= 0
195 
196 % Range for contact point in finger 4
197 beta_4:= pi/2
198 
199 
200 % BEZIER PATCHES !!
201 % 4 contacts, pillada por debajo
202 % ring patch
203 b1_00_x_o := 51.188
204 b1_00_y_o := 34.125
205 b1_00_z_o := 75.644
206 b1_01_x_o := 63.222
207 b1_01_y_o := 50.068
208 b1_01_z_o := 63.222
209 b1_10_x_o := 34.950
210 b1_10_y_o := 50.068
211 b1_10_z_o := 82.144
212 b1_11_x_o := 47.189
213 b1_11_y_o := 66.970
214 b1_11_z_o := 69.734
215 
216 
217 % middle patch
218 b2_00_x_o := 0
219 b2_00_y_o := 34.125
220 b2_00_z_o := 91
221 b2_01_x_o := 18.143
222 b2_01_y_o := 50.068
223 b2_01_z_o := 87.264
224 b2_10_x_o := -18.143
225 b2_10_y_o := 50.068
226 b2_10_z_o := 87.264
227 b2_11_x_o := 0
228 b2_11_y_o := 66.970
229 b2_11_z_o := 83.891
230 
231 % index patch
232 b3_00_x_o := -51.188
233 b3_00_y_o := 34.125
234 b3_00_z_o := 75.644
235 b3_01_x_o := -34.95
236 b3_01_y_o := 50.068
237 b3_01_z_o := 82.144
238 b3_10_x_o := -63.222
239 b3_10_y_o := 50.068
240 b3_10_z_o := 63.222
241 b3_11_x_o := -47.189
242 b3_11_y_o := 66.97
243 b3_11_z_o := 69.734
244 
245 % thumb patch
246 b4_00_x_o := -51.188
247 b4_00_y_o := 34.125
248 b4_00_z_o := -75.644
249 b4_01_x_o := -63.222
250 b4_01_y_o := 50.068
251 b4_01_z_o := -63.222
252 b4_10_x_o := -34.950
253 b4_10_y_o := 50.068
254 b4_10_z_o := -82.144
255 b4_11_x_o := -47.189
256 b4_11_y_o := 66.970
257 b4_11_z_o := -69.734
258 
259 % palm patch
260 bp_00_x_o := 38.391
261 bp_00_y_o := 0
262 bp_00_z_o := 56.733
263 bp_01_x_o := -56.733
264 bp_01_y_o := 0
265 bp_01_z_o := 38.391
266 bp_10_x_o := 56.733
267 bp_10_y_o := 0
268 bp_10_z_o := -38.391
269 bp_11_x_o := -38.391
270 bp_11_y_o := 0
271 bp_11_z_o := -56.733
272 
273 
274 
275 %-------------------------------------------------------------------
276 
277 %--------------------------------------------------------------------
278 
279 
280 [LINKS]
281 
282 % palm
283  palm: body "bodies/palmSAH.off" white
284  body "bodies/acs1palm.off" blue
285  body "bodies/acs2palm.off" blue
286  body "bodies/acs3palm.off"
287 
288 % finger 1
289  p12: body "bodies/p1.off" white
290  body "bodies/p2.off" white
291  body "bodies/b2.off" blue
292 
293  p13: body "bodies/p3.off" white
294 
295  p14: body "bodies/p4.off" white
296  body "bodies/b4.off" blue
297 
298 % finger 2
299  p22: body "bodies/p1.off" white
300  body "bodies/p2.off" white
301  body "bodies/b2.off" blue
302 
303  p23: body "bodies/p3.off" white
304 
305  p24: body "bodies/p4.off" white
306  body "bodies/b4.off" blue
307 
308 % finger 3
309  p32: body "bodies/p1.off" white
310  body "bodies/p2.off" white
311  body "bodies/b2.off" blue
312 
313  p33: body "bodies/p3.off" white
314 
315  p34: body "bodies/p4.off" white
316  body "bodies/b4.off" blue
317 
318 % finger 4
319  p40: body "bodies/p0thumb.off" white
320 
321  p42: body "bodies/p1.off" white
322  body "bodies/p2.off" white
323  body "bodies/b2.off" blue
324 
325  p43: body "bodies/p3.off" white
326 
327  p44: body "bodies/p4.off" white
328  body "bodies/b4.off" blue
329 
330  object: body "bodies/teapot.off" red
331 %cyan sphere RS (0,0,0)
332 
333  X_P: green sphere rc (0,0,0)
334 
335  X_1: green sphere rc (0,0,0)
336 
337  X_2: green sphere rc (0,0,0)
338 
339  X_3: green sphere rc (0,0,0)
340 
341 % X_4: green sphere rc (0,0,0)
342 
343 
344 
345 [JOINTS]
346 
347 % finger 1
348  universal: palm (q11x,q11y,q11z)+(1,0,0)
349  p12 (0,0,0)+(0,1,0)
350  range [phi_1_1_min,phi_1_1_max] +(0,-0.99941,0.034301)
351  [phi_1_2_min,phi_1_2_max] +(0,0,1)
352  avoid limits
353 
354  revolute: p12 (q13x_2,q13y_2,q13z_2)+(0,-1,0)
355  p13 (0,0,0)+(0,-1,0)
356  range [phi_1_3_min,phi_1_3_max] +(1,0,0) +(1,0,0)
357  avoid limits
358 
359  revolute: p13 (q14x_3,q14y_3,q14z_3)+(0,-1,0)
360  p14 (0,0,0)+(0,-1,0)
361  range [phi_1_4_min,phi_1_4_max] +(1,0,0) +(1,0,0)
362  avoid limits
363 
364 % finger 2
365  universal: palm (q21x,q21y,q21z)+(1,0,0)
366  p22 (0,0,0)+(0,1,0)
367  range [phi_2_1_min,phi_2_1_max] +(0,-1,0)
368  [phi_2_2_min,phi_2_2_max] +(0, 0,1)
369  avoid limits
370 
371  revolute: p22 (q23x_2,q23y_2,q23z_2)+(0,-1,0)
372  p23 (0,0,0)+(0,-1,0)
373  range [phi_2_3_min,phi_2_3_max] +(1,0,0) +(1,0,0)
374  avoid limits
375 
376  revolute: p23 (q24x_3,q24y_3,q24z_3)+(0,-1,0)
377  p24 (0,0,0)+(0,-1,0)
378  range [phi_2_4_min,phi_2_4_max] +(1,0,0) +(1,0,0)
379  avoid limits
380 
381 % finger 3
382  universal: palm (q31x,q31y,q31z)+(1,0,0)
383  p32 (0,0,0)+(0,1,0)
384  range [phi_3_1_min,phi_3_1_max] +(0,-0.99937,-0.035485)
385  [phi_3_2_min,phi_3_2_max] +(0, 0,1)
386  avoid limits
387 
388  revolute: p32 (q33x_2,q33y_2,q33z_2)+(0,-1,0)
389  p33 (0,0,0)+(0,-1,0)
390  range [phi_3_3_min,phi_3_3_max] +(1,0,0) +(1,0,0)
391  avoid limits
392 
393  revolute: p33 (q34x_3,q34y_3,q34z_3)+(0,-1,0)
394  p34 (0,0,0)+(0,-1,0)
395  range [phi_3_4_min,phi_3_4_max] +(1,0,0) +(1,0,0)
396  avoid limits
397 
398 % finger 4
399  revolute: palm (q40x,q40y,q40z)+(0,0,-1)
400  p40 (0,0,0)+(-1,0,0)
401  range [phi_4_0_min,phi_4_0_max] +(0,-1,0) +(0,0,1)
402  %avoid limits
403 
404  universal: p40 (q41x_0,q41y_0,q41z_0)+(0.573580,0,0.81915)
405  p42 (0,0,0)+(0,1,0)
406  range [phi_4_1_min,phi_4_1_max] +(0,1,0)
407  [phi_4_2_min,phi_4_2_max] +(0,0,1)
408  %avoid limits
409 
410  revolute: p42 (q43x_2,q43y_2,q43z_2)+(0,-1,0)
411  p43 (0,0,0)+(0,-1,0)
412  range [phi_4_3_min,phi_4_3_max] +(1,0,0) +(1,0,0)
413  %avoid limits
414 
415  revolute: p43 (q44x_3,q44y_3,q44z_3)+(0,-1,0)
416  p44 (0,0,0)+(0,-1,0)
417  range [phi_4_4_min,phi_4_4_max] +(1,0,0) +(1,0,0)
418  %avoid limits
419 
420 % KINEMATIC OF THE OBJECT-FINGER CONTACT
421 
422 
423  SPHERICAL: p14 (q15x_4,q15y_4,q15z_4)
424  X_1 (0,0,-rho)
425  range [beta_1] +(m1x_4,m1y_4,m1z_4) +(0,0,1)
426  avoid limits
427 
428  in_patch: X_1 (0,0,0) (0,0,1)
429  object (b1_00_x_o,b1_00_y_o,b1_00_z_o) (b1_01_x_o,b1_01_y_o,b1_01_z_o) (b1_10_x_o,b1_10_y_o,b1_10_z_o) (b1_11_x_o,b1_11_y_o,b1_11_z_o)
430  avoid limits
431 
432  SPHERICAL: p24 (q25x_4,q25y_4,q25z_4)
433  X_2 (0,0,-rho)
434  range [beta_2] +(m2x_4,m2y_4,m2z_4) +(0,0,1)
435  avoid limits
436 
437  in_patch: X_2 (0,0,0) (0,0,1)
438  object (b2_00_x_o,b2_00_y_o,b2_00_z_o) (b2_01_x_o,b2_01_y_o,b2_01_z_o) (b2_10_x_o,b2_10_y_o,b2_10_z_o) (b2_11_x_o,b2_11_y_o,b2_11_z_o)
439  avoid limits
440 
441  SPHERICAL: p34 (q35x_4,q35y_4,q35z_4)
442  X_3 (0,0,-rho)
443  range [beta_3] +(m3x_4,m3y_4,m3z_4) +(0,0,1)
444  avoid limits
445 
446  in_patch: X_3 (0,0,0) (0,0,1)
447  object (b3_00_x_o,b3_00_y_o,b3_00_z_o) (b3_01_x_o,b3_01_y_o,b3_01_z_o) (b3_10_x_o,b3_10_y_o,b3_10_z_o) (b3_11_x_o,b3_11_y_o,b3_11_z_o)
448  avoid limits
449 
450 % SPHERICAL: p44 (q45x_4,q45y_4,q45z_4)
451 % X_4 (0,0,-rho)
452 % range [beta_4] +(m4x_4,m4y_4,m4z_4) +(0,0,1)
453 % avoid limits
454 
455 % in_patch: X_4 (0,0,0) (0,0,1)
456 % object (b4_00_x_o,b4_00_y_o,b4_00_z_o) (b4_01_x_o,b4_01_y_o,b4_01_z_o) (b4_10_x_o,b4_10_y_o,b4_10_z_o) (b4_11_x_o,b4_11_y_o,b4_11_z_o)
457 % avoid limits
458 
459  SPHERICAL: palm (qpx,qpy,qpz)
460  X_P (0,0,-rho)
461  range [beta_1] +(mpx,mpy,mpz) +(0,0,1)
462  avoid limits
463 
464  in_patch: X_P (0,0,0) (0,0,1)
465  object (bp_00_x_o,bp_00_y_o,bp_00_z_o) (bp_01_x_o,bp_01_y_o,bp_01_z_o) (bp_10_x_o,bp_10_y_o,bp_10_z_o) (bp_11_x_o,bp_11_y_o,bp_11_z_o)
466  avoid limits
467 
468 
469 
470 
471