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:=13
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 := -10
82 q15z_4 := 0
83 m1x_4 := 0.14
84 m1y_4 := 0
85 m1z_4 := 0.99
86 
87 
88 %q25x_4 := 3.852
89 %q25y_4 := -12.189+rho
90 %q25z_4 := -11.268
91 %m2x_4 := 0
92 %m2y_4 := -1
93 %m2z_4 := 0
94 
95 q25x_4 := 20
96 q25y_4 := 0
97 q25z_4 := 0
98 m2x_4 := 0.14
99 m2y_4 := 0
100 m2z_4 := 0.99
101 
102 
103 q34x_3_c := 14.619
104 q34y_3_c := -0.012
105 q34z_3_c := 12.859-rho
106 m3x_3 := 0
107 m3y_3 := 0
108 m3z_3 := 1
109 
110 q35x_4 := 20
111 q35y_4 := 10
112 q35z_4 := 0
113 m3x_4 := 0.14
114 m3y_4 := 0
115 m3z_4 := 0.99
116 
117 q45x_4 := 20
118 q45y_4 := 0
119 q45z_4 := 0
120 m4x_4 := 0.14
121 m4y_4 := 0
122 m4z_4 := 0.99
123 
124 
125 % Palm contact points vectors, and normal vector.
126 %point 1
127 qpx := 20.550-1*rho
128 qpy := -0.053
129 qpz := 117.348
130 mpx := 1
131 mpy := 0
132 mpz := 0
133 
134 beta_p := pi/3
135 
136 % Joint ranges
137 
138 % Two ranges for the universal joint connecting the palm and finger 1
139 phi_1_1_min:= 0
140 phi_1_1_max:= 0
141 
142 phi_1_2_min:= 0
143 phi_1_2_max:= 0
144 
145 % Range for the first rotation in finger 1
146 phi_1_3_min:= 0
147 phi_1_3_max:= 0
148 
149 % Range for the second rotation in finger 1
150 phi_1_4_min:= 0
151 phi_1_4_max:= 0
152 
153 % Range for contact point in finger 1
154 beta_1:= pi/2
155 
156 % Two ranges for the universal joint connecting the palm and finger 2
157 phi_2_1_min:= 0
158 phi_2_1_max:= 0
159 
160 phi_2_2_min:= 0
161 phi_2_2_max:= 0
162 
163 % Range for the first rotation in finger 2
164 phi_2_3_min:= 0
165 phi_2_3_max:= 0
166 
167 % Range for the second rotation in finger 2
168 phi_2_4_min:= 0
169 phi_2_4_max:= 0
170 
171 % Range for contact point in finger 2
172 beta_2:= pi/2
173 
174 % Two ranges for the universal joint connecting the palm and finger 3
175 phi_3_1_min:= -pi/8
176 phi_3_1_max:= pi/8
177 
178 phi_3_2_min:= 0
179 phi_3_2_max:= pi/2
180 
181 % Range for the first rotation in finger 3
182 phi_3_3_min:= 0
183 phi_3_3_max:= pi/2
184 
185 % Range for the second rotation in finger 3
186 phi_3_4_min:= 0
187 phi_3_4_max:= pi/2
188 
189 % Range for contact point in finger 3
190 beta_3:= pi/3
191 
192 % Two ranges for the universal joint connecting the palm and finger 4
193 phi_4_0_min:=0
194 phi_4_0_max:= pi/2
195 
196 phi_4_1_min:= -pi/8
197 phi_4_1_max:= pi/8
198 
199 phi_4_2_min:= 0
200 phi_4_2_max:= pi/2
201 
202 % Range for the first rotation in finger 4
203 phi_4_3_min:= 0
204 phi_4_3_max:= pi/2
205 
206 % Range for the second rotation in finger 4
207 phi_4_4_min:= 0
208 phi_4_4_max:= pi/2
209 
210 % Range for contact point in finger 4
211 beta_4:= pi/3
212 
213 
214 % BEZIER PATCHES !!
215 % 2 contacts (pillada por el tapa)
216 % index patch
217 b3_00_x_o := 6.493
218 b3_00_y_o := 131.821
219 b3_00_z_o := -15.208
220 b3_10_x_o := 3.152
221 b3_10_y_o := 133.041
222 b3_10_z_o := -15.373
223 b3_01_x_o := 3.079
224 b3_01_y_o := 130.322
225 b3_01_z_o := -15.030
226 b3_11_x_o := 0
227 b3_11_y_o := 131.821
228 b3_11_z_o := -16.476
229 
230 % thumb patch
231 b4_00_x_o := -6.493
232 b4_00_y_o := 131.821
233 b4_00_z_o := 15.208
234 b4_01_x_o := -3.079
235 b4_01_y_o := 130.322
236 b4_01_z_o := 15.030
237 b4_10_x_o := -3.152
238 b4_10_y_o := 133.041
239 b4_10_z_o := 15.373
240 b4_11_x_o := 0
241 b4_11_y_o := 131.821
242 b4_11_z_o := 16.476
243 
244 
245 %-------------------------------------------------------------------
246 
247 %--------------------------------------------------------------------
248 
249 
250 [LINKS]
251 
252 % palm
253  palm: body "bodies/palmSAH.off" white
254  body "bodies/acs1palm.off" blue
255  body "bodies/acs2palm.off" blue
256  body "bodies/acs3palm.off"
257 
258 % finger 1
259  p12: body "bodies/p1.off" white
260  body "bodies/p2.off" white
261  body "bodies/b2.off" blue
262 
263  p13: body "bodies/p3.off" white
264 
265  p14: body "bodies/p4.off" white
266  body "bodies/b4.off" blue
267 
268 % finger 2
269  p22: body "bodies/p1.off" white
270  body "bodies/p2.off" white
271  body "bodies/b2.off" blue
272 
273  p23: body "bodies/p3.off" white
274 
275  p24: body "bodies/p4.off" white
276  body "bodies/b4.off" blue
277 
278 % finger 3
279  p32: body "bodies/p1.off" white
280  body "bodies/p2.off" white
281  body "bodies/b2.off" blue
282 
283  p33: body "bodies/p3.off" white
284 
285  p34: body "bodies/p4.off" white
286  body "bodies/b4.off" blue
287 
288 % finger 4
289  p40: body "bodies/p0thumb.off" white
290 
291  p42: body "bodies/p1.off" white
292  body "bodies/p2.off" white
293  body "bodies/b2.off" blue
294 
295  p43: body "bodies/p3.off" white
296 
297  p44: body "bodies/p4.off" white
298  body "bodies/b4.off" blue
299 
300  object: body "bodies/teapot.off" white
301 %cyan sphere RS (0,0,0)
302 
303 % X_P: green sphere rc (0,0,0)
304 
305 % X_1: green sphere rc (0,0,0)
306 
307 % X_2: green sphere rc (0,0,0)
308 
309  X_3: green sphere rc (0,0,0)
310 
311  X_4: green sphere rc (0,0,0)
312 
313 
314 
315 [JOINTS]
316 
317 % finger 1
318  universal: palm (q11x,q11y,q11z)+(1,0,0)
319  p12 (0,0,0)+(0,1,0)
320  range [phi_1_1_min,phi_1_1_max] +(0,-0.99941,0.034301)
321  [phi_1_2_min,phi_1_2_max] +(0,0,1)
322  avoid limits
323 
324  revolute: p12 (q13x_2,q13y_2,q13z_2)+(0,-1,0)
325  p13 (0,0,0)+(0,-1,0)
326  range [phi_1_3_min,phi_1_3_max] +(1,0,0) +(1,0,0)
327  avoid limits
328 
329  revolute: p13 (q14x_3,q14y_3,q14z_3)+(0,-1,0)
330  p14 (0,0,0)+(0,-1,0)
331  range [phi_1_4_min,phi_1_4_max] +(1,0,0) +(1,0,0)
332  avoid limits
333 
334 % finger 2
335  universal: palm (q21x,q21y,q21z)+(1,0,0)
336  p22 (0,0,0)+(0,1,0)
337  range [phi_2_1_min,phi_2_1_max] +(0,-1,0)
338  [phi_2_2_min,phi_2_2_max] +(0, 0,1)
339  avoid limits
340 
341  revolute: p22 (q23x_2,q23y_2,q23z_2)+(0,-1,0)
342  p23 (0,0,0)+(0,-1,0)
343  range [phi_2_3_min,phi_2_3_max] +(1,0,0) +(1,0,0)
344  avoid limits
345 
346  revolute: p23 (q24x_3,q24y_3,q24z_3)+(0,-1,0)
347  p24 (0,0,0)+(0,-1,0)
348  range [phi_2_4_min,phi_2_4_max] +(1,0,0) +(1,0,0)
349  avoid limits
350 
351 % finger 3
352  universal: palm (q31x,q31y,q31z)+(1,0,0)
353  p32 (0,0,0)+(0,1,0)
354  range [phi_3_1_min,phi_3_1_max] +(0,-0.99937,-0.035485)
355  [phi_3_2_min,phi_3_2_max] +(0, 0,1)
356  avoid limits
357 
358  revolute: p32 (q33x_2,q33y_2,q33z_2)+(0,-1,0)
359  p33 (0,0,0)+(0,-1,0)
360  range [phi_3_3_min,phi_3_3_max] +(1,0,0) +(1,0,0)
361  avoid limits
362 
363  revolute: p33 (q34x_3,q34y_3,q34z_3)+(0,-1,0)
364  p34 (0,0,0)+(0,-1,0)
365  range [phi_3_4_min,phi_3_4_max] +(1,0,0) +(1,0,0)
366  avoid limits
367 
368 % finger 4
369  revolute: palm (q40x,q40y,q40z)+(0,0,-1)
370  p40 (0,0,0)+(-1,0,0)
371  range [phi_4_0_min,phi_4_0_max] +(0,-1,0) +(0,0,1)
372  avoid limits
373 
374  universal: p40 (q41x_0,q41y_0,q41z_0)+(0.573580,0,0.81915)
375  p42 (0,0,0)+(0,1,0)
376  range [phi_4_1_min,phi_4_1_max] +(0,1,0)
377  [phi_4_2_min,phi_4_2_max] +(0,0,1)
378  avoid limits
379 
380  revolute: p42 (q43x_2,q43y_2,q43z_2)+(0,-1,0)
381  p43 (0,0,0)+(0,-1,0)
382  range [phi_4_3_min,phi_4_3_max] +(1,0,0) +(1,0,0)
383  avoid limits
384 
385  revolute: p43 (q44x_3,q44y_3,q44z_3)+(0,-1,0)
386  p44 (0,0,0)+(0,-1,0)
387  range [phi_4_4_min,phi_4_4_max] +(1,0,0) +(1,0,0)
388  avoid limits
389 
390 % KINEMATIC OF THE OBJECT-FINGER CONTACT
391 
392 
393 % SPHERICAL: p14 (q15x_4,q15y_4,q15z_4)
394 % X_1 (0,0,-rho)
395 % range [beta_1] +(m1x_4,m1y_4,m1z_4) +(0,0,1)
396 % avoid limits
397 
398 % in_patch: X_1 (0,0,0) (0,0,1)
399 % 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)
400 % avoid limits
401 
402 % SPHERICAL: p24 (q25x_4,q25y_4,q25z_4)
403 % X_2 (0,0,-rho)
404 % range [beta_2] +(m2x_4,m2y_4,m2z_4) +(0,0,1)
405 % avoid limits
406 
407 % in_patch: X_2 (0,0,0) (0,0,1)
408 % 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)
409 % avoid limits
410 
411  SPHERICAL: p34 (q35x_4,q35y_4,q35z_4)
412  X_3 (0,0,-rho)
413  range [beta_3] +(m3x_4,m3y_4,m3z_4) +(0,0,1)
414  avoid limits
415 
416  in_patch: X_3 (0,0,0) (0,0,1)
417  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)
418  avoid limits
419 
420  SPHERICAL: p44 (q45x_4,q45y_4,q45z_4)
421  X_4 (0,0,-rho)
422  range [beta_4] +(m4x_4,m4y_4,m4z_4) +(0,0,1)
423  avoid limits
424 
425  in_patch: X_4 (0,0,0) (0,0,1)
426  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)
427  avoid limits
428 
429 % SPHERICAL: palm (qpx,qpy,qpz)
430 % X_P (0,0,-rho)
431 % range [beta_1] +(mpx,mpy,mpz) +(0,0,1)
432 % avoid limits
433 
434 % in_patch: X_P (0,0,0) (0,0,1)
435 % 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)
436 % avoid limits
437 
438 
439 
440 
441