tro.world
Go to the documentation of this file.
1 
2 [CONSTANTS]
3 
4 % qji_k is a vector expreseing the anchor point of reference
5 % frame i with respect to reference frame k of finger j.
6 % if k is not given, it means it is respect to the palm,
7 % which in our case, it is the absoulute one.
8 
9 q11x := -4.3
10 q11y := -40.165
11 q11z := 145.43
12 %q11 = q12
13 
14 q21x := -4.3
15 q21y := 0
16 q21z := 150.15
17 %q21 = q22
18 
19 q31x := -4.3
20 q31y := 40.165
21 q31z := 145.43
22 %q31 = q32
23 
24 q40x := -3
25 q40y := 27.1
26 q40z := 0
27 
28 q41x_0:= 97
29 q41y_0:= 6
30 q41z_0:=-87
31 %q41_0 = q42_0
32 
33 
34 % These anchor points can be seen as the length of the phalanges,
35 % but that is because the phalanges are aligned with this vectors.
36 q13x_2 := 67.8
37 q13y_2 := 0
38 q13z_2 := 0
39 
40 q14x_3 := 30
41 q14y_3 := 0
42 q14z_3 := 0
43 
44 
45 q23x_2 := 67.8
46 q23y_2 := 0
47 q23z_2 := 0
48 
49 q24x_3 := 30
50 q24y_3 := 0
51 q24z_3 := 0
52 
53 
54 q33x_2 := 67.8
55 q33y_2 := 0
56 q33z_2 := 0
57 
58 q34x_3 := 30
59 q34y_3 := 0
60 q34z_3 := 0
61 
62 
63 q43x_2 := 67.8
64 q43y_2 := 0
65 q43z_2 := 0
66 
67 q44x_3 := 30
68 q44y_3 := 0
69 q44z_3 := 0
70 
71 
72 % Hand-object contact points (plus normals)
73 % The points are numbered as 1,2,3,4 and
74 % those in the object must contact the corresponding
75 % one in the hand.
76 % The contact points in the hand are at the end of
77 % fingers 1,2,3 and 4 (4=thumb).
78 % All contact points are given in local coordinates of
79 % the corresponding bodies.
80 
81 op1_x:= 0.8524959031868207
82 op1_y:= -43.2394688661446338
83 op1_z:= 34.7689835132430289
84 
85 op2_x:= 8.9593048181687323
86 op2_y:= -12.4983060535382968
87 op2_z:= 34.6462743220432685
88 
89 op3_x:= 7.7097462921607018
90 op3_y:= 29.0467630975741216
91 op3_z:= 34.4423366856801039
92 
93 op4_x:= 6.1545806145491326
94 op4_y:= 22.5808710603167029
95 op4_z:= -32.3682807959821588
96 
97 on1_x:= 0
98 on1_y:= 0
99 on1_z:= 1
100 
101 on2_x:= 0
102 on2_y:= 0
103 on2_z:= 1
104 
105 on3_x:= 0
106 on3_y:= 0
107 on3_z:= 1
108 
109 on4_x:= 0
110 on4_y:= 0
111 on4_z:=-1
112 
113 % Four contact points, one per finger tip
114 hp1_x:= 30.0471075309416875
115 hp1_y:= -1.2365419354376943
116 hp1_z:= 12.6454634566004813
117 
118 hp2_x:= 34.7388633310358728
119 hp2_y:= 2.7634773254394531
120 hp2_z:= 9.8875802900529379
121 
122 hp3_x:= 33.4480110741168630
123 hp3_y:= -2.4118313487914307
124 hp3_z:= 10.6210256259656717
125 
126 hp4_x:= 33.2014357274337897
127 hp4_y:= 0.4581835731986708
128 hp4_z:= 10.6962855370975980
129 
130 % Normal at the fingertip
131 hn1_x:= -0.5364353326414127
132 hn1_y:= 0.1124632789203126
133 hn1_z:= -0.8364144575438595
134 
135 hn2_x:= -0.5308589401000310
136 hn2_y:= 0.0845936279070145
137 hn2_z:= -0.8432275516332476
138 
139 hn3_x:= -0.3358913267009292
140 hn3_y:= 0.0683072136824264
141 hn3_z:= -0.9394206412497187
142 
143 hn4_x:= -0.5126593648173801
144 hn4_y:= -0.0907476069112436
145 hn4_z:= -0.8537829042004312
146 
147 % Joint ranges
148 
149 % We can symmetrically enlarge the ranges for the joints so that
150 % the hand can better manipulate objects without regrasping.
151 % Set to zero to use the original ranges
152 enlarge_range := 30*pi/180
153 
154 % Two ranges for the universal joint connecting the palm and finger 1
155 % -pi/16
156 phi_1_1_min:= -pi/8 -enlarge_range
157 phi_1_1_max:= pi/8 +enlarge_range
158 
159 % 1.35*pi/16
160 phi_1_2_min:= 0 -enlarge_range
161 phi_1_2_max:= pi/2 +enlarge_range
162 
163 % Range for the first rotation in finger 2
164 % pi/8
165 phi_1_3_min:= 0 -enlarge_range
166 phi_1_3_max:= pi/2 +enlarge_range
167 
168 % Range for the second rotation in finger 2
169 % pi/8
170 phi_1_4_min:= 0 -enlarge_range
171 phi_1_4_max:= pi/2 +enlarge_range
172 
173 
174 % Two ranges for the universal joint connecting the palm and finger 2
175 % -pi/16
176 phi_2_1_min:= -pi/8 -enlarge_range
177 phi_2_1_max:= pi/8 +enlarge_range
178 
179 % 1.35*pi/16
180 phi_2_2_min:= 0 -enlarge_range
181 phi_2_2_max:= pi/2 +enlarge_range
182 
183 % Range for the first rotation in finger 2
184 % pi/8
185 phi_2_3_min:= 0 -enlarge_range
186 phi_2_3_max:= pi/2 +enlarge_range
187 
188 % Range for the second rotation in finger 2
189 % pi/8
190 phi_2_4_min:= 0 -enlarge_range
191 phi_2_4_max:= pi/2 +enlarge_range
192 
193 % Two ranges for the universal joint connecting the palm and finger 3
194 % -pi/16
195 phi_3_1_min:= -pi/8 -enlarge_range
196 phi_3_1_max:= pi/8 +enlarge_range
197 
198 % 1.2*pi/16
199 phi_3_2_min:= 0 -enlarge_range
200 phi_3_2_max:= pi/2 +enlarge_range
201 
202 % Range for the first rotation in finger 3
203 % pi/8
204 phi_3_3_min:= 0 -enlarge_range
205 phi_3_3_max:= pi/2 +enlarge_range
206 
207 % Range for the second rotation in finger 3
208 % pi/8
209 phi_3_4_min:= 0 -enlarge_range
210 phi_3_4_max:= pi/2 +enlarge_range
211 
212 % Two ranges for the universal joint connecting the palm and finger 4
213 % pi/2
214 phi_4_0_min:= 0 -enlarge_range
215 phi_4_0_max:= pi/2 +enlarge_range
216 
217 % 0
218 phi_4_1_min:= -pi/8 -enlarge_range
219 phi_4_1_max:= pi/8 +enlarge_range
220 
221 % 0
222 phi_4_2_min:= 0 -enlarge_range
223 phi_4_2_max:= pi/2 +enlarge_range
224 
225 % Range for the first rotation in finger 4
226 % 3*pi/16
227 phi_4_3_min:= 0 -enlarge_range
228 phi_4_3_max:= pi/2 +enlarge_range
229 
230 % Range for the second rotation in finger 4
231 % 3*pi/16
232 phi_4_4_min:= 0 -enlarge_range
233 phi_4_4_max:= pi/2 +enlarge_range
234 
235 
236 %-------------------------------------------------------------------
237 %--------------------------------------------------------------------
238 
239 [LINKS]
240 
241 % palm
242  palm:body "bodies/palmSAH.off" white
243  body "bodies/acs1palm.off" blue
244  body "bodies/acs2palm.off" blue
245  body "bodies/acs3palm.off"
246 /*
247  cylinder 5 (300,-60,800) (300,-10,800) red
248  cylinder 5 (300,-60,800) (250,-60,800) red
249  cylinder 5 (300,-10,800) (250,-10,800) red
250  sphere 5 (300,-60,800) red
251  sphere 5 (300,-10,800) red
252  sphere 5 (250,-60,800) red
253  sphere 5 (250,-10,800) red
254 */
255 
256  cylinder 5 (350,60,800) (350,10,800) blue
257  cylinder 5 (350,60,800) (400,60,800) blue
258  cylinder 5 (350,10,800) (400,10,800) blue
259  sphere 5 (350,60,800) blue
260  sphere 5 (350,10,800) blue
261  sphere 5 (400,60,800) blue
262  sphere 5 (400,10,800) blue
263 
264 
265 %% finger 1
266  p12: body "bodies/p1.off" white
267  body "bodies/p2.off" white
268  body "bodies/b2.off" blue
269 
270  p13: body "bodies/p3.off" white
271 
272  p14: body "bodies/p4.off" white
273  body "bodies/b4.off" blue
274 
275  p15 % : sphere 4 (0,0,hp1_z)
276  % cylinder 2 (0,0,hp1_z)+(100*hn1_x,100*hn1_y,100*hn1_z) red decoration
277 
278 % finger 2
279  p22: body "bodies/p1.off" white
280  body "bodies/p2.off" white
281  body "bodies/b2.off" blue
282 
283  p23: body "bodies/p3.off" white
284 
285  p24: body "bodies/p4.off" white
286  body "bodies/b4.off" blue
287 
288  p25 % : sphere 4 (0,0,hp2_z)
289  % cylinder 2 (0,0,hp2_z)+(100*hn2_x,100*hn2_y,100*hn2_z) red decoration
290 
291 % finger 3
292  p32: body "bodies/p1.off" white
293  body "bodies/p2.off" white
294  body "bodies/b2.off" blue
295 
296  p33: body "bodies/p3.off" white
297 
298  p34: body "bodies/p4.off" white
299  body "bodies/b4.off" blue
300 
301  p35 % : sphere 4 (0,0,hp3_z)
302  % cylinder 2 (0,0,hp3_z)+(100*hn3_x,100*hn3_y,100*hn3_z) red decoration
303 
304 % finger 4
305  p40: body "bodies/p0thumb.off" white
306 
307  p42: body "bodies/p1.off" white
308  body "bodies/p2.off" white
309  body "bodies/b2.off" blue
310 
311  p43: body "bodies/p3.off" white
312 
313  p44: body "bodies/p4.off" white
314  body "bodies/b4.off" blue
315 
316  p45 % : sphere 4 (0,0,hp4_z)
317  % cylinder 2 (0,0,hp4_z)+(100*hn4_x,100*hn4_y,100*hn4_z) red decoration
318 
319 %object
320  object: body "bodies/needle2.off" green
321  % cylinder 2 (op1_x,op1_y,op1_z)+(100*on1_x,100*on1_y,100*on1_z) blue decoration
322  % cylinder 2 (op2_x,op2_y,op2_z)+(100*on2_x,100*on2_y,100*on2_z) blue decoration
323  % cylinder 2 (op3_x,op3_y,op3_z)+(100*on3_x,100*on3_y,100*on3_z) blue decoration
324  % cylinder 2 (op4_x,op4_y,op4_z)+(100*on4_x,100*on4_y,100*on4_z) blue decoration
325 
326 [JOINTS]
327 
328 %% finger 1
329  universal: palm (q11x,q11y,q11z)+(1,0,0)
330  p12 (0,0,0)+(0,1,0)
331  range [phi_1_1_min,phi_1_1_max] +(0,-0.99941,0.034301)
332  [phi_1_2_min,phi_1_2_max] +(1,0,0)
333 
334  revolute: p12 (q13x_2,q13y_2,q13z_2)+(0,-1,0)
335  p13 (0,0,0)+(0,-1,0)
336  range [phi_1_3_min,phi_1_3_max] +(1,0,0) +(1,0,0)
337 
338  crevolute: p13 (q14x_3,q14y_3,q14z_3)+(0,-1,0)
339  p14 (0,0,0)+(0,-1,0)
340  range [phi_1_4_min,phi_1_4_max] +(1,0,0) +(1,0,0)
341 
342 
343 % finger 2
344  universal: palm (q21x,q21y,q21z)+(1,0,0)
345  p22 (0,0,0)+(0,1,0)
346  range [phi_2_1_min,phi_2_1_max] +(0,-1,0)
347  [phi_2_2_min,phi_2_2_max] +(1, 0,0)
348 
349  revolute: p22 (q23x_2,q23y_2,q23z_2)+(0,-1,0)
350  p23 (0,0,0)+(0,-1,0)
351  range [phi_2_3_min,phi_2_3_max] +(1,0,0) +(1,0,0)
352 
353  crevolute: p23 (q24x_3,q24y_3,q24z_3)+(0,-1,0)
354  p24 (0,0,0)+(0,-1,0)
355  range [phi_2_4_min,phi_2_4_max] +(1,0,0) +(1,0,0)
356 
357 
358 % finger 3
359  universal: palm (q31x,q31y,q31z)+(1,0,0)
360  p32 (0,0,0)+(0,1,0)
361  range [phi_3_1_min,phi_3_1_max] +(0,-0.99937,-0.035485)
362  [phi_3_2_min,phi_3_2_max] +(1, 0,0)
363 
364  revolute: p32 (q33x_2,q33y_2,q33z_2)+(0,-1,0)
365  p33 (0,0,0)+(0,-1,0)
366  range [phi_3_3_min,phi_3_3_max] +(1,0,0) +(1,0,0)
367 
368  crevolute: p33 (q34x_3,q34y_3,q34z_3)+(0,-1,0)
369  p34 (0,0,0)+(0,-1,0)
370  range [phi_3_4_min,phi_3_4_max] +(1,0,0) +(1,0,0)
371 
372 
373 % finger 4
374  revolute: palm (q40x,q40y,q40z)+(0,0,-1)
375  p40 (0,0,0)+(-1,0,0)
376  range [phi_4_0_min,phi_4_0_max] +(0,-1,0) +(0,0,1)
377 
378  universal: p40 (q41x_0,q41y_0,q41z_0)+(0.573580,0,0.81915)
379  p42 (0,0,0)+(0,1,0)
380  range [phi_4_1_min,phi_4_1_max] +(0,1,0)
381  [phi_4_2_min,phi_4_2_max] +(1,0,0)
382 
383  revolute: p42 (q43x_2,q43y_2,q43z_2)+(0,-1,0)
384  p43 (0,0,0)+(0,-1,0)
385  range [phi_4_3_min,phi_4_3_max] +(1,0,0) +(1,0,0)
386 
387  revolute: p43 (q44x_3,q44y_3,q44z_3)+(0,-1,0)
388  p44 (0,0,0)+(0,-1,0)
389  range [phi_4_4_min,phi_4_4_max] +(1,0,0) +(1,0,0)
390 
391 
392 % Now we define the hand-object connection via revolute joints
393 
394  revolute: p14 (hp1_x-5,hp1_y,0)+(0,1,0)
395  p15 (0,0,0)+(0,1,0)
396  range [0,pi/2] +(0,0,1) +(0,0,1)
397 
398  revolute: p15 (0,0,hp1_z)+(hn1_x,hn1_y,hn1_z)
399  object (op1_x,op1_y,op1_z)+(on1_x,on1_y,on1_z)
400 
401  revolute: p24 (hp2_x-5,hp2_y,0)+(0,1,0)
402  p25 (0,0,0)+(0,1,0)
403  range [0,pi/2] +(0,0,1) +(0,0,1)
404 
405  revolute: p25 (0,0,hp2_z)+(hn2_x,hn2_y,hn2_z)
406  object (op2_x,op2_y,op2_z)+(on2_x,on2_y,on2_z)
407 
408  revolute: p34 (hp3_x-5,hp3_y,0)+(0,1,0)
409  p35 (0,0,0)+(0,1,0)
410  range [0,pi/2] +(0,0,1) +(0,0,1)
411 
412  revolute: p35 (0,0,hp3_z)+(hn3_x,hn3_y,hn3_z)
413  object (op3_x,op3_y,op3_z)+(on3_x,on3_y,on3_z)
414 
415  revolute: p44 (hp4_x-5,hp4_y,0)+(0,1,0)
416  p45 (0,0,0)+(0,1,0)
417  range [0,pi/2] +(0,0,1) +(0,0,1)
418 
419  revolute: p45 (0,0,hp4_z)+(hn4_x,hn4_y,hn4_z)
420  object (op4_x,op4_y,op4_z)+(on4_x,on4_y,on4_z)
421 
422 
423 % This the fix transformation to the object used to defined the grasping
424 % fix: palm object Tx(25) * Ry(-pi/4) *Tz(120)* Tx(180) * Ty(35)
425 
426 
427 % [OBSTACLES]
428 
429  % obs1: body "bodies/obstacle1" color (0.9,0.9,0.1)
430  % obs2: body "bodies/obstacle2" color (0.9,0.9,0.1)
431  % obs3: body "bodies/obstacle3" color (0.9,0.9,0.1)
432  % % obs4: body "bodies/obstacle4" color (0.9,0.9,0.1)
433  % obs4: body "bodies/obstacle4" color (0.9,0.9,0.1)
434 
435 
436 [COLLISIONS]
437 
438  Do Not Check: all
439  Check: object,palm
440 
441  /*
442  Check: object,obs1
443  Check: object,obs2
444  Check: object,obs3
445  Check: object,obs4
446 
447  Check: object,p14
448  Check: object,p24
449  Check: object,p34
450  Check: object,p44
451  */
452