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