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.
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.
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
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.
81 op1_x:= 0.8524959031868207
82 op1_y:= -43.2394688661446338
83 op1_z:= 34.7689835132430289
85 op2_x:= 8.9593048181687323
86 op2_y:= -12.4983060535382968
87 op2_z:= 34.6462743220432685
89 op3_x:= 7.7097462921607018
90 op3_y:= 29.0467630975741216
91 op3_z:= 34.4423366856801039
93 op4_x:= 6.1545806145491326
94 op4_y:= 22.5808710603167029
95 op4_z:= -32.3682807959821588
113 % Four contact points, one per finger tip
114 hp1_x:= 30.0471075309416875
115 hp1_y:= -1.2365419354376943
116 hp1_z:= 12.6454634566004813
118 hp2_x:= 34.7388633310358728
119 hp2_y:= 2.7634773254394531
120 hp2_z:= 9.8875802900529379
122 hp3_x:= 33.4480110741168630
123 hp3_y:= -2.4118313487914307
124 hp3_z:= 10.6210256259656717
126 hp4_x:= 33.2014357274337897
127 hp4_y:= 0.4581835731986708
128 hp4_z:= 10.6962855370975980
130 % Normal at the fingertip
131 hn1_x:= -0.5364353326414127
132 hn1_y:= 0.1124632789203126
133 hn1_z:= -0.8364144575438595
135 hn2_x:= -0.5308589401000310
136 hn2_y:= 0.0845936279070145
137 hn2_z:= -0.8432275516332476
139 hn3_x:= -0.3358913267009292
140 hn3_y:= 0.0683072136824264
141 hn3_z:= -0.9394206412497187
143 hn4_x:= -0.5126593648173801
144 hn4_y:= -0.0907476069112436
145 hn4_z:= -0.8537829042004312
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
154 % Two ranges
for the universal joint connecting the palm and finger 1
156 phi_1_1_min:= -pi/8 -enlarge_range
157 phi_1_1_max:= pi/8 +enlarge_range
160 phi_1_2_min:= 0 -enlarge_range
161 phi_1_2_max:= pi/2 +enlarge_range
163 % Range
for the first rotation in finger 2
165 phi_1_3_min:= 0 -enlarge_range
166 phi_1_3_max:= pi/2 +enlarge_range
168 % Range
for the second rotation in finger 2
170 phi_1_4_min:= 0 -enlarge_range
171 phi_1_4_max:= pi/2 +enlarge_range
174 % Two ranges
for the universal joint connecting the palm and finger 2
176 phi_2_1_min:= -pi/8 -enlarge_range
177 phi_2_1_max:= pi/8 +enlarge_range
180 phi_2_2_min:= 0 -enlarge_range
181 phi_2_2_max:= pi/2 +enlarge_range
183 % Range
for the first rotation in finger 2
185 phi_2_3_min:= 0 -enlarge_range
186 phi_2_3_max:= pi/2 +enlarge_range
188 % Range
for the second rotation in finger 2
190 phi_2_4_min:= 0 -enlarge_range
191 phi_2_4_max:= pi/2 +enlarge_range
193 % Two ranges
for the universal joint connecting the palm and finger 3
195 phi_3_1_min:= -pi/8 -enlarge_range
196 phi_3_1_max:= pi/8 +enlarge_range
199 phi_3_2_min:= 0 -enlarge_range
200 phi_3_2_max:= pi/2 +enlarge_range
202 % Range
for the first rotation in finger 3
204 phi_3_3_min:= 0 -enlarge_range
205 phi_3_3_max:= pi/2 +enlarge_range
207 % Range
for the second rotation in finger 3
209 phi_3_4_min:= 0 -enlarge_range
210 phi_3_4_max:= pi/2 +enlarge_range
212 % Two ranges
for the universal joint connecting the palm and finger 4
214 phi_4_0_min:= 0 -enlarge_range
215 phi_4_0_max:= pi/2 +enlarge_range
218 phi_4_1_min:= -pi/8 -enlarge_range
219 phi_4_1_max:= pi/8 +enlarge_range
222 phi_4_2_min:= 0 -enlarge_range
223 phi_4_2_max:= pi/2 +enlarge_range
225 % Range
for the first rotation in finger 4
227 phi_4_3_min:= 0 -enlarge_range
228 phi_4_3_max:= pi/2 +enlarge_range
230 % Range
for the second rotation in finger 4
232 phi_4_4_min:= 0 -enlarge_range
233 phi_4_4_max:= pi/2 +enlarge_range
236 %-------------------------------------------------------------------
237 %--------------------------------------------------------------------
242 palm:body
"bodies/palmSAH.off" white
243 body
"bodies/acs1palm.off" blue
244 body
"bodies/acs2palm.off" blue
245 body
"bodies/acs3palm.off"
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
266 p12: body "bodies/p1.off" white
267 body "bodies/p2.off" white
268 body "bodies/b2.off" blue
270 p13: body "bodies/p3.off" white
272 p14: body "bodies/p4.off" white
273 body "bodies/b4.off" blue
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
279 p22: body "bodies/p1.off" white
280 body "bodies/p2.off" white
281 body "bodies/b2.off" blue
283 p23: body "bodies/p3.off" white
285 p24: body "bodies/p4.off" white
286 body "bodies/b4.off" blue
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
292 p32: body "bodies/p1.off" white
293 body "bodies/p2.off" white
294 body "bodies/b2.off" blue
296 p33: body "bodies/p3.off" white
298 p34: body "bodies/p4.off" white
299 body "bodies/b4.off" blue
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
305 p40: body "bodies/p0thumb.off" white
307 p42: body "bodies/p1.off" white
308 body "bodies/p2.off" white
309 body "bodies/b2.off" blue
311 p43: body "bodies/p3.off" white
313 p44: body "bodies/p4.off" white
314 body "bodies/b4.off" blue
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
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
329 universal: palm (q11x,q11y,q11z)+(1,0,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)
334 revolute: p12 (q13x_2,q13y_2,q13z_2)+(0,-1,0)
336 range [phi_1_3_min,phi_1_3_max] +(1,0,0) +(1,0,0)
338 crevolute: p13 (q14x_3,q14y_3,q14z_3)+(0,-1,0)
340 range [phi_1_4_min,phi_1_4_max] +(1,0,0) +(1,0,0)
344 universal: palm (q21x,q21y,q21z)+(1,0,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)
349 revolute: p22 (q23x_2,q23y_2,q23z_2)+(0,-1,0)
351 range [phi_2_3_min,phi_2_3_max] +(1,0,0) +(1,0,0)
353 crevolute: p23 (q24x_3,q24y_3,q24z_3)+(0,-1,0)
355 range [phi_2_4_min,phi_2_4_max] +(1,0,0) +(1,0,0)
359 universal: palm (q31x,q31y,q31z)+(1,0,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)
364 revolute: p32 (q33x_2,q33y_2,q33z_2)+(0,-1,0)
366 range [phi_3_3_min,phi_3_3_max] +(1,0,0) +(1,0,0)
368 crevolute: p33 (q34x_3,q34y_3,q34z_3)+(0,-1,0)
370 range [phi_3_4_min,phi_3_4_max] +(1,0,0) +(1,0,0)
374 revolute: palm (q40x,q40y,q40z)+(0,0,-1)
376 range [phi_4_0_min,phi_4_0_max] +(0,-1,0) +(0,0,1)
378 universal: p40 (q41x_0,q41y_0,q41z_0)+(0.573580,0,0.81915)
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)
383 revolute: p42 (q43x_2,q43y_2,q43z_2)+(0,-1,0)
385 range [phi_4_3_min,phi_4_3_max] +(1,0,0) +(1,0,0)
387 revolute: p43 (q44x_3,q44y_3,q44z_3)+(0,-1,0)
389 range [phi_4_4_min,phi_4_4_max] +(1,0,0) +(1,0,0)
392 % Now we define the hand-
object connection via revolute joints
394 revolute: p14 (hp1_x-5,hp1_y,0)+(0,1,0)
396 range [0,pi/2] +(0,0,1) +(0,0,1)
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)
401 revolute: p24 (hp2_x-5,hp2_y,0)+(0,1,0)
403 range [0,pi/2] +(0,0,1) +(0,0,1)
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)
408 revolute: p34 (hp3_x-5,hp3_y,0)+(0,1,0)
410 range [0,pi/2] +(0,0,1) +(0,0,1)
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)
415 revolute: p44 (hp4_x-5,hp4_y,0)+(0,1,0)
417 range [0,pi/2] +(0,0,1) +(0,0,1)
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)
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)
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)
Follow us!