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