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.
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.
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
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.
83 op1_x:= 0.8524959031868207
84 op1_y:= -43.2394688661446338
85 op1_z:= 34.7689835132430289
87 op2_x:= 8.9593048181687323
88 op2_y:= -12.4983060535382968
89 op2_z:= 34.6462743220432685
91 op3_x:= 7.7097462921607018
92 op3_y:= 29.0467630975741216
93 op3_z:= 34.4423366856801039
95 op4_x:= 6.1545806145491326
96 op4_y:= 22.5808710603167029
97 op4_z:= -32.3682807959821588
115 % Four contact points, one per finger tip
116 hp1_x:= 30.0471075309416875
117 hp1_y:= -1.2365419354376943
118 hp1_z:= 12.6454634566004813
120 hp2_x:= 34.7388633310358728
121 hp2_y:= 2.7634773254394531
122 hp2_z:= 9.8875802900529379
124 hp3_x:= 33.4480110741168630
125 hp3_y:= -2.4118313487914307
126 hp3_z:= 10.6210256259656717
128 hp4_x:= 33.2014357274337897
129 hp4_y:= 0.4581835731986708
130 hp4_z:= 10.6962855370975980
132 % Normal at the fingertip
133 hn1_x:= -0.5364353326414127
134 hn1_y:= 0.1124632789203126
135 hn1_z:= -0.8364144575438595
137 hn2_x:= -0.5308589401000310
138 hn2_y:= 0.0845936279070145
139 hn2_z:= -0.8432275516332476
141 hn3_x:= -0.3358913267009292
142 hn3_y:= 0.0683072136824264
143 hn3_z:= -0.9394206412497187
145 hn4_x:= -0.5126593648173801
146 hn4_y:= -0.0907476069112436
147 hn4_z:= -0.8537829042004312
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
156 % Two ranges
for the universal joint connecting the palm and finger 1
158 phi_1_1_min:= -pi/8 -enlarge_range
159 phi_1_1_max:= pi/8 +enlarge_range
162 phi_1_2_min:= 0 -enlarge_range
163 phi_1_2_max:= pi/2 +enlarge_range
165 % Range
for the first rotation in finger 2
167 phi_1_3_min:= 0 -enlarge_range
168 phi_1_3_max:= pi/2 +enlarge_range
170 % Range
for the second rotation in finger 2
172 phi_1_4_min:= 0 -enlarge_range
173 phi_1_4_max:= pi/2 +enlarge_range
176 % Two ranges
for the universal joint connecting the palm and finger 2
178 phi_2_1_min:= -pi/8 -enlarge_range
179 phi_2_1_max:= pi/8 +enlarge_range
182 phi_2_2_min:= 0 -enlarge_range
183 phi_2_2_max:= pi/2 +enlarge_range
185 % Range
for the first rotation in finger 2
187 phi_2_3_min:= 0 -enlarge_range
188 phi_2_3_max:= pi/2 +enlarge_range
190 % Range
for the second rotation in finger 2
192 phi_2_4_min:= 0 -enlarge_range
193 phi_2_4_max:= pi/2 +enlarge_range
195 % Two ranges
for the universal joint connecting the palm and finger 3
197 phi_3_1_min:= -pi/8 -enlarge_range
198 phi_3_1_max:= pi/8 +enlarge_range
201 phi_3_2_min:= 0 -enlarge_range
202 phi_3_2_max:= pi/2 +enlarge_range
204 % Range
for the first rotation in finger 3
206 phi_3_3_min:= 0 -enlarge_range
207 phi_3_3_max:= pi/2 +enlarge_range
209 % Range
for the second rotation in finger 3
211 phi_3_4_min:= 0 -enlarge_range
212 phi_3_4_max:= pi/2 +enlarge_range
214 % Two ranges
for the universal joint connecting the palm and finger 4
216 phi_4_0_min:= 0 -enlarge_range
217 phi_4_0_max:= pi/2 +enlarge_range
220 phi_4_1_min:= -pi/8 -enlarge_range
221 phi_4_1_max:= pi/8 +enlarge_range
224 phi_4_2_min:= 0 -enlarge_range
225 phi_4_2_max:= pi/2 +enlarge_range
227 % Range
for the first rotation in finger 4
229 phi_4_3_min:= 0 -enlarge_range
230 phi_4_3_max:= pi/2 +enlarge_range
232 % Range
for the second rotation in finger 4
234 phi_4_4_min:= 0 -enlarge_range
235 phi_4_4_max:= pi/2 +enlarge_range
238 %-------------------------------------------------------------------
239 %--------------------------------------------------------------------
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
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
263 palm:body "bodies/palmSAH.off" white
264 body "bodies/acs1palm.off" blue
265 body "bodies/acs2palm.off" blue
266 body "bodies/acs3palm.off"
269 p12: body "bodies/p1.off" white
270 body "bodies/p2.off" white
271 body "bodies/b2.off" blue
273 p13: body "bodies/p3.off" white
275 p14: body "bodies/p4.off" white
276 body "bodies/b4.off" blue
278 p15 % : sphere 4 (0,0,hp1_z)
279 % cylinder 2 (0,0,hp1_z)+(100*hn1_x,100*hn1_y,100*hn1_z) red decoration
282 p22: body "bodies/p1.off" white
283 body "bodies/p2.off" white
284 body "bodies/b2.off" blue
286 p23: body "bodies/p3.off" white
288 p24: body "bodies/p4.off" white
289 body "bodies/b4.off" blue
291 p25 % : sphere 4 (0,0,hp2_z)
292 % cylinder 2 (0,0,hp2_z)+(100*hn2_x,100*hn2_y,100*hn2_z) red decoration
295 p32: body "bodies/p1.off" white
296 body "bodies/p2.off" white
297 body "bodies/b2.off" blue
299 p33: body "bodies/p3.off" white
301 p34: body "bodies/p4.off" white
302 body "bodies/b4.off" blue
304 p35 % : sphere 4 (0,0,hp3_z)
305 % cylinder 2 (0,0,hp3_z)+(100*hn3_x,100*hn3_y,100*hn3_z) red decoration
308 p40: body "bodies/p0thumb.off" white
310 p42: body "bodies/p1.off" white
311 body "bodies/p2.off" white
312 body "bodies/b2.off" blue
314 p43: body "bodies/p3.off" white
316 p44: body "bodies/p4.off" white
317 body "bodies/b4.off" blue
319 p45 % : sphere 4 (0,0,hp4_z)
320 % cylinder 2 (0,0,hp4_z)+(100*hn4_x,100*hn4_y,100*hn4_z) red decoration
323 object: body "bodies/needle2.off" green
324 % cylinder 2 (op1_x,op1_y,op1_z)+(100*on1_x,100*on1_y,100*on1_z) blue decoration
325 % cylinder 2 (op2_x,op2_y,op2_z)+(100*on2_x,100*on2_y,100*on2_z) blue decoration
326 % cylinder 2 (op3_x,op3_y,op3_z)+(100*on3_x,100*on3_y,100*on3_z) blue decoration
327 % cylinder 2 (op4_x,op4_y,op4_z)+(100*on4_x,100*on4_y,100*on4_z) blue decoration
331 %% Global rotation of the hand at the wrist
332 revolute: world (0,0,0)+(0,0,1)
334 range [-10*
DEG2RAD,10*DEG2RAD] +(1,0,0) +(1,0,0)
337 universal: palm (q11x,q11y,q11z)+(1,0,0)
339 range [phi_1_1_min,phi_1_1_max] +(0,-0.99941,0.034301)
340 [phi_1_2_min,phi_1_2_max] +(1,0,0)
342 revolute: p12 (q13x_2,q13y_2,q13z_2)+(0,-1,0)
344 range [phi_1_3_min,phi_1_3_max] +(1,0,0) +(1,0,0)
346 crevolute: p13 (q14x_3,q14y_3,q14z_3)+(0,-1,0)
348 range [phi_1_4_min,phi_1_4_max] +(1,0,0) +(1,0,0)
352 universal: palm (q21x,q21y,q21z)+(1,0,0)
354 range [phi_2_1_min,phi_2_1_max] +(0,-1,0)
355 [phi_2_2_min,phi_2_2_max] +(1, 0,0)
357 revolute: p22 (q23x_2,q23y_2,q23z_2)+(0,-1,0)
359 range [phi_2_3_min,phi_2_3_max] +(1,0,0) +(1,0,0)
361 crevolute: p23 (q24x_3,q24y_3,q24z_3)+(0,-1,0)
363 range [phi_2_4_min,phi_2_4_max] +(1,0,0) +(1,0,0)
367 universal: palm (q31x,q31y,q31z)+(1,0,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] +(1, 0,0)
372 revolute: p32 (q33x_2,q33y_2,q33z_2)+(0,-1,0)
374 range [phi_3_3_min,phi_3_3_max] +(1,0,0) +(1,0,0)
376 crevolute: p33 (q34x_3,q34y_3,q34z_3)+(0,-1,0)
378 range [phi_3_4_min,phi_3_4_max] +(1,0,0) +(1,0,0)
382 revolute: palm (q40x,q40y,q40z)+(0,0,-1)
384 range [phi_4_0_min,phi_4_0_max] +(0,-1,0) +(0,0,1)
386 universal: p40 (q41x_0,q41y_0,q41z_0)+(0.573580,0,0.81915)
388 range [phi_4_1_min,phi_4_1_max] +(0,1,0)
389 [phi_4_2_min,phi_4_2_max] +(1,0,0)
391 revolute: p42 (q43x_2,q43y_2,q43z_2)+(0,-1,0)
393 range [phi_4_3_min,phi_4_3_max] +(1,0,0) +(1,0,0)
395 revolute: p43 (q44x_3,q44y_3,q44z_3)+(0,-1,0)
397 range [phi_4_4_min,phi_4_4_max] +(1,0,0) +(1,0,0)
400 % Now we define the hand-
object connection via revolute joints
402 revolute: p14 (hp1_x-5,hp1_y,0)+(0,1,0)
404 range [0,pi/2] +(0,0,1) +(0,0,1)
406 revolute: p15 (0,0,hp1_z)+(hn1_x,hn1_y,hn1_z)
407 object (op1_x,op1_y,op1_z)+(on1_x,on1_y,on1_z)
409 revolute: p24 (hp2_x-5,hp2_y,0)+(0,1,0)
411 range [0,pi/2] +(0,0,1) +(0,0,1)
413 revolute: p25 (0,0,hp2_z)+(hn2_x,hn2_y,hn2_z)
414 object (op2_x,op2_y,op2_z)+(on2_x,on2_y,on2_z)
416 revolute: p34 (hp3_x-5,hp3_y,0)+(0,1,0)
418 range [0,pi/2] +(0,0,1) +(0,0,1)
420 revolute: p35 (0,0,hp3_z)+(hn3_x,hn3_y,hn3_z)
421 object (op3_x,op3_y,op3_z)+(on3_x,on3_y,on3_z)
423 revolute: p44 (hp4_x-5,hp4_y,0)+(0,1,0)
425 range [0,pi/2] +(0,0,1) +(0,0,1)
427 revolute: p45 (0,0,hp4_z)+(hn4_x,hn4_y,hn4_z)
428 object (op4_x,op4_y,op4_z)+(on4_x,on4_y,on4_z)
431 % This the fix transformation to the
object used to defined the grasping
432 % fix: palm
object Tx(25) * Ry(-pi/4) *Tz(120)* Tx(180) * Ty(35)
437 % obs1: body "bodies/obstacle1" color (0.9,0.9,0.1)
438 % obs2: body "bodies/obstacle2" color (0.9,0.9,0.1)
439 % obs3: body "bodies/obstacle3" color (0.9,0.9,0.1)
440 % % obs4: body "bodies/obstacle4" color (0.9,0.9,0.1)
441 % obs4: body "bodies/obstacle4" color (0.9,0.9,0.1)
#define DEG2RAD
Constant to transform from degrees to radiants.
Follow us!