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
264 palm:body "bodies/palmSAH.off" white
265 body "bodies/acs1palm.off" blue
266 body "bodies/acs2palm.off" blue
267 body "bodies/acs3palm.off"
270 p12: body "bodies/p1.off" white
271 body "bodies/p2.off" white
272 body "bodies/b2.off" blue
274 p13: body "bodies/p3.off" white
276 p14: body "bodies/p4.off" white
277 body "bodies/b4.off" blue
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
283 p22: body "bodies/p1.off" white
284 body "bodies/p2.off" white
285 body "bodies/b2.off" blue
287 p23: body "bodies/p3.off" white
289 p24: body "bodies/p4.off" white
290 body "bodies/b4.off" blue
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
296 p32: body "bodies/p1.off" white
297 body "bodies/p2.off" white
298 body "bodies/b2.off" blue
300 p33: body "bodies/p3.off" white
302 p34: body "bodies/p4.off" white
303 body "bodies/b4.off" blue
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
309 p40: body "bodies/p0thumb.off" white
311 p42: body "bodies/p1.off" white
312 body "bodies/p2.off" white
313 body "bodies/b2.off" blue
315 p43: body "bodies/p3.off" white
317 p44: body "bodies/p4.off" white
318 body "bodies/b4.off" blue
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
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
332 %% Global rotation of the hand at the wrist
333 revolute: world (0,0,0)+(0,0,1)
335 range [-10*
DEG2RAD,10*DEG2RAD] +(1,0,0) +(1,0,0)
338 universal: palm (q11x,q11y,q11z)+(1,0,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)
343 revolute: p12 (q13x_2,q13y_2,q13z_2)+(0,-1,0)
345 range [phi_1_3_min,phi_1_3_max] +(1,0,0) +(1,0,0)
347 crevolute: p13 (q14x_3,q14y_3,q14z_3)+(0,-1,0)
349 range [phi_1_4_min,phi_1_4_max] +(1,0,0) +(1,0,0)
353 universal: palm (q21x,q21y,q21z)+(1,0,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)
358 revolute: p22 (q23x_2,q23y_2,q23z_2)+(0,-1,0)
360 range [phi_2_3_min,phi_2_3_max] +(1,0,0) +(1,0,0)
362 crevolute: p23 (q24x_3,q24y_3,q24z_3)+(0,-1,0)
364 range [phi_2_4_min,phi_2_4_max] +(1,0,0) +(1,0,0)
368 universal: palm (q31x,q31y,q31z)+(1,0,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)
373 revolute: p32 (q33x_2,q33y_2,q33z_2)+(0,-1,0)
375 range [phi_3_3_min,phi_3_3_max] +(1,0,0) +(1,0,0)
377 crevolute: p33 (q34x_3,q34y_3,q34z_3)+(0,-1,0)
379 range [phi_3_4_min,phi_3_4_max] +(1,0,0) +(1,0,0)
383 revolute: palm (q40x,q40y,q40z)+(0,0,-1)
385 range [phi_4_0_min,phi_4_0_max] +(0,-1,0) +(0,0,1)
387 universal: p40 (q41x_0,q41y_0,q41z_0)+(0.573580,0,0.81915)
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)
392 revolute: p42 (q43x_2,q43y_2,q43z_2)+(0,-1,0)
394 range [phi_4_3_min,phi_4_3_max] +(1,0,0) +(1,0,0)
396 revolute: p43 (q44x_3,q44y_3,q44z_3)+(0,-1,0)
398 range [phi_4_4_min,phi_4_4_max] +(1,0,0) +(1,0,0)
401 % Now we define the hand-
object connection via revolute joints
403 revolute: p14 (hp1_x-5,hp1_y,0)+(0,1,0)
405 range [0,pi/2] +(0,0,1) +(0,0,1)
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)
410 revolute: p24 (hp2_x-5,hp2_y,0)+(0,1,0)
412 range [0,pi/2] +(0,0,1) +(0,0,1)
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)
417 revolute: p34 (hp3_x-5,hp3_y,0)+(0,1,0)
419 range [0,pi/2] +(0,0,1) +(0,0,1)
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)
424 revolute: p44 (hp4_x-5,hp4_y,0)+(0,1,0)
426 range [0,pi/2] +(0,0,1) +(0,0,1)
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)
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)
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)
#define DEG2RAD
Constant to transform from degrees to radiants.
Follow us!