SAHand.world
Go to the documentation of this file.
1 
2 [CONSTANTS]
3 
4 % qji_k is a vector expressing the anchor point of reference frame i with respect to reference frame k of finger j.
5 % if k is not given, it means it is respect to the palm, which in our case, it is the absoulute one.
6 
7 q11x := -4.3
8 q11y := -40.165
9 q11z := 145.43
10 %q11 = q12
11 
12 q21x := -4.3
13 q21y := 0
14 q21z := 150.15
15 %q21 = q22
16 
17 q31x := -4.3
18 q31y := 40.165
19 q31z := 145.43
20 %q31 = q32
21 
22 q40x := -3
23 q40y := 27.1
24 q40z := 0
25 
26 q41x_0:= 97
27 q41y_0:= 6
28 q41z_0:=-87
29 %q41_0 = q42_0
30 
31 
32 % These anchor points can be seen as the length of the phalanges,
33 % but that is because the phalanges are aligned with this vectors.
34 q13x_2 := 67.8
35 q13y_2 := 0
36 q13z_2 := 0
37 
38 q14x_3 := 30
39 q14y_3 := 0
40 q14z_3 := 0
41 
42 
43 q23x_2 := 67.8
44 q23y_2 := 0
45 q23z_2 := 0
46 
47 q24x_3 := 30
48 q24y_3 := 0
49 q24z_3 := 0
50 
51 
52 q33x_2 := 67.8
53 q33y_2 := 0
54 q33z_2 := 0
55 
56 q34x_3 := 30
57 q34y_3 := 0
58 q34z_3 := 0
59 
60 
61 q43x_2 := 67.8
62 q43y_2 := 0
63 q43z_2 := 0
64 
65 q44x_3 := 30
66 q44y_3 := 0
67 q44z_3 := 0
68 
69 
70 % Fingertip point vectors, qj5_4, and fingertip normal vector, mj_4
71 q15x_4 := 25
72 q15y_4 := 0
73 q15z_4 := 0
74 m1x_4 := 0.707
75 m1y_4 := 0
76 m1z_4 := 0.707
77 
78 q25x_4 := q15x_4
79 q25y_4 := q15y_4
80 q25z_4 := q15z_4
81 m2x_4 := m1x_4
82 m2y_4 := m1y_4
83 m2z_4 := m1z_4
84 
85 q35x_4 := q15x_4
86 q35y_4 := q15y_4
87 q35z_4 := q15z_4
88 m3x_4 := m1x_4
89 m3y_4 := m1y_4
90 m3z_4 := m1z_4
91 
92 q45x_4 := q15x_4
93 q45y_4 := q15y_4
94 q45z_4 := q15z_4
95 m4x_4 := m1x_4
96 m4y_4 := m1y_4
97 m4z_4 := m1z_4
98 
99 
100 % Joint ranges
101 
102 % Two ranges for the universal joint connecting the palm and finger 1
103 phi_1_1_min:=-pi/8
104 phi_1_1_max:= pi/8
105 
106 phi_1_2_min:= 0
107 phi_1_2_max:= pi/2
108 
109 % Range for the first rotation in finger 1
110 phi_1_3_min:= 0
111 phi_1_3_max:= pi/2
112 
113 % Range for the second rotation in finger 1
114 phi_1_4_min:= 0
115 phi_1_4_max:= pi/2
116 
117 % Range for contact point in finger 1
118 beta_1:= pi/6
119 
120 % Two ranges for the universal joint connecting the palm and finger 2
121 phi_2_1_min:= phi_1_1_min
122 phi_2_1_max:= phi_1_1_max
123 
124 phi_2_2_min:= phi_1_2_min
125 phi_2_2_max:= phi_1_2_max
126 
127 % Range for the first rotation in finger 2
128 phi_2_3_min:= phi_1_3_min
129 phi_2_3_max:= phi_1_3_max
130 
131 % Range for the second rotation in finger 2
132 phi_2_4_min:= phi_1_4_min
133 phi_2_4_max:= phi_1_4_max
134 
135 % Range for contact point in finger 2
136 beta_2:= beta_1
137 
138 % Two ranges for the universal joint connecting the palm and finger 3
139 phi_3_1_min:= phi_1_1_min
140 phi_3_1_max:= phi_1_1_max
141 
142 phi_3_2_min:= phi_1_2_min
143 phi_3_2_max:= phi_1_2_max
144 
145 % Range for the first rotation in finger 2
146 phi_3_3_min:= phi_1_3_min
147 phi_3_3_max:= phi_1_3_max
148 
149 % Range for the second rotation in finger 2
150 phi_3_4_min:= phi_1_4_min
151 phi_3_4_max:= phi_1_4_max
152 
153 % Range for contact point in finger 3
154 beta_3:= beta_1
155 
156 % Two ranges for the universal joint connecting the palm and finger 4
157 phi_4_0_min:= 0
158 phi_4_0_max:= pi
159 
160 phi_4_1_min:= phi_1_1_min
161 phi_4_1_max:= phi_1_1_max
162 
163 phi_4_2_min:= phi_1_2_min
164 phi_4_2_max:= phi_1_2_max
165 
166 % Range for the first rotation in finger 2
167 phi_4_3_min:= phi_1_3_min
168 phi_4_3_max:= phi_1_3_max
169 
170 % Range for the second rotation in finger 2
171 phi_4_4_min:= phi_1_4_min
172 phi_4_4_max:= phi_1_4_max
173 
174 % Range for contact point in finger 4
175 beta_4:= beta_1
176 
177 
178 
179 % Radius of the spheres representing contact points on the object
180 rc:=5
181 
182 % radius of the sphere modeling the fingertip
183 rho:=13
184 
185 % INPUT TO OUR PROBLEM
186 %--------------------------------------------------------------------
187 % Object points (xj) and inward normals (nj) in object frame
188 % This is the output of the grasp planning algorithm
189 % The change on the contact model, makes us to extend the contact
190 % point along the normal at that point. (xj_e)
191 %--------------------------------------------------------------------
192 n1x_o := 0
193 n1y_o := -1
194 n1z_o := 0
195 x1x_o := 0
196 x1y_o := 0
197 x1z_o := 0
198 x1x_o_e := x1x_o - rho*n1x_o
199 x1y_o_e := x1y_o - rho*n1y_o
200 x1z_o_e := x1z_o - rho*n1z_o
201 x1x_o_i := x1x_o + rho*n1x_o
202 x1y_o_i := x1y_o + rho*n1y_o
203 x1z_o_i := x1z_o + rho*n1z_o
204 
205 n2x_o := 0
206 n2y_o := -1
207 n2z_o := 0
208 x2x_o := 40
209 x2y_o := 0
210 x2z_o := 0
211 x2x_o_e := x2x_o - rho*n2x_o
212 x2y_o_e := x2y_o - rho*n2y_o
213 x2z_o_e := x2z_o - rho*n2z_o
214 x2x_o_i := x2x_o + rho*n2x_o
215 x2y_o_i := x2y_o + rho*n2y_o
216 x2z_o_i := x2z_o + rho*n2z_o
217 
218 n3x_o := 0
219 n3y_o := -1
220 n3z_o := 0
221 x3x_o := 80
222 x3y_o := 0
223 x3z_o := 0
224 x3x_o_e := x3x_o - rho*n3x_o
225 x3y_o_e := x3y_o - rho*n3y_o
226 x3z_o_e := x3z_o - rho*n3z_o
227 x3x_o_i := x3x_o + rho*n3x_o
228 x3y_o_i := x3y_o + rho*n3y_o
229 x3z_o_i := x3z_o + rho*n3z_o
230 
231 n4x_o := 0
232 n4y_o := 1
233 n4z_o := 0
234 x4x_o := 40
235 x4y_o := -40
236 x4z_o := 0
237 x4x_o_e := x4x_o - rho*n4x_o
238 x4y_o_e := x4y_o - rho*n4y_o
239 x4z_o_e := x4z_o - rho*n4z_o
240 x4x_o_i := x4x_o + rho*n4x_o
241 x4y_o_i := x4y_o + rho*n4y_o
242 x4z_o_i := x4z_o + rho*n4z_o
243 
244 
245 
246 %-------------------------------------------------------------------
247 
248 %--------------------------------------------------------------------
249 
250 
251 [LINKS]
252 
253 % palm
254  palm: body "bodies/palmSAH.off" white
255  body "bodies/acs1palm.off" blue
256  body "bodies/acs2palm.off" blue
257  body "bodies/acs3palm.off"
258 
259 % finger 1
260  p12: body "bodies/p1.off" white
261  body "bodies/p2.off" white
262  body "bodies/b2.off" blue
263 
264  p13: body "bodies/p3.off" white
265 
266  p14: body "bodies/p4.off" white
267  body "bodies/b4.off" blue
268 
269 % finger 2
270  p22: body "bodies/p1.off" white
271  body "bodies/p2.off" white
272  body "bodies/b2.off" blue
273 
274  p23: body "bodies/p3.off" white
275 
276  p24: body "bodies/p4.off" white
277  body "bodies/b4.off" blue
278 
279 % finger 3
280  p32: body "bodies/p1.off" white
281  body "bodies/p2.off" white
282  body "bodies/b2.off" blue
283 
284  p33: body "bodies/p3.off" white
285 
286  p34: body "bodies/p4.off" white
287  body "bodies/b4.off" blue
288 
289 % finger 4
290  p40: body "bodies/p0thumb.off" white
291 
292  p42: body "bodies/p1.off" white
293  body "bodies/p2.off" white
294  body "bodies/b2.off" blue
295 
296  p43: body "bodies/p3.off" white
297 
298  p44: body "bodies/p4.off" white
299  body "bodies/b4.off" blue
300 
301 
302 % object: one small circle at the contact points plus cylinders
303 % connecting the contact points (representing the object body).
304  object: 0.75*red
305  sphere rc (x1x_o,x1y_o,x1z_o)
306  sphere rc (x2x_o,x2y_o,x2z_o)
307  sphere rc (x3x_o,x3y_o,x3z_o)
308  sphere rc (x4x_o,x4y_o,x4z_o)
309  cylinder rc*0.25 (x1x_o,x1y_o,x1z_o) (x1x_o_i,x1y_o_i,x1z_o_i)
310  cylinder rc*0.25 (x2x_o,x2y_o,x2z_o) (x2x_o_i,x2y_o_i,x2z_o_i)
311  cylinder rc*0.25 (x3x_o,x3y_o,x3z_o) (x3x_o_i,x3y_o_i,x3z_o_i)
312  cylinder rc*0.25 (x4x_o,x4y_o,x4z_o) (x4x_o_i,x4y_o_i,x4z_o_i)
313 
314  cylinder rc*0.5 (x1x_o,x1y_o,x1z_o) (x2x_o,x2y_o,x2z_o)
315  cylinder rc*0.5 (x1x_o,x1y_o,x1z_o) (x3x_o,x3y_o,x3z_o)
316  cylinder rc*0.5 (x1x_o,x1y_o,x1z_o) (x4x_o,x4y_o,x4z_o)
317  cylinder rc*0.5 (x2x_o,x2y_o,x2z_o) (x3x_o,x3y_o,x3z_o)
318  cylinder rc*0.5 (x2x_o,x2y_o,x2z_o) (x4x_o,x4y_o,x4z_o)
319  cylinder rc*0.5 (x3x_o,x3y_o,x3z_o) (x4x_o,x4y_o,x4z_o)
320 
321 [JOINTS]
322 
323 % finger 1
324  universal: palm (q11x,q11y,q11z)+(1,0,0)
325  p12 (0,0,0)+(0,1,0)
326  range [phi_1_1_min,phi_1_1_max] +(0,-0.99941,0.034301)
327  [phi_1_2_min,phi_1_2_max] +(1,0,0)
328 
329  revolute: p12 (q13x_2,q13y_2,q13z_2)+(0,-1,0)
330  p13 (0,0,0)+(0,-1,0)
331  range [phi_1_3_min,phi_1_3_max] +(1,0,0) +(1,0,0)
332 
333  revolute: p13 (q14x_3,q14y_3,q14z_3)+(0,-1,0)
334  p14 (0,0,0)+(0,-1,0)
335  range [phi_1_4_min,phi_1_4_max] +(1,0,0) +(1,0,0)
336 
337 % finger 2
338  universal: palm (q21x,q21y,q21z)+(1,0,0)
339  p22 (0,0,0)+(0,1,0)
340  range [phi_2_1_min,phi_2_1_max] +(0,-1,0)
341  [phi_2_2_min,phi_2_2_max] +(1, 0,0)
342 
343  revolute: p22 (q23x_2,q23y_2,q23z_2)+(0,-1,0)
344  p23 (0,0,0)+(0,-1,0)
345  range [phi_2_3_min,phi_2_3_max] +(1,0,0) +(1,0,0)
346 
347  revolute: p23 (q24x_3,q24y_3,q24z_3)+(0,-1,0)
348  p24 (0,0,0)+(0,-1,0)
349  range [phi_2_4_min,phi_2_4_max] +(1,0,0) +(1,0,0)
350 
351 % finger 3
352  universal: palm (q31x,q31y,q31z)+(1,0,0)
353  p32 (0,0,0)+(0,1,0)
354  range [phi_3_1_min,phi_3_1_max] +(0,-0.99937,-0.035485)
355  [phi_3_2_min,phi_3_2_max] +(1, 0,0)
356 
357  revolute: p32 (q33x_2,q33y_2,q33z_2)+(0,-1,0)
358  p33 (0,0,0)+(0,-1,0)
359  range [phi_3_3_min,phi_3_3_max] +(1,0,0) +(1,0,0)
360 
361  revolute: p33 (q34x_3,q34y_3,q34z_3)+(0,-1,0)
362  p34 (0,0,0)+(0,-1,0)
363  range [phi_3_4_min,phi_3_4_max] +(1,0,0) +(1,0,0)
364 
365 % finger 4
366  revolute: palm (q40x,q40y,q40z)+(0,0,-1)
367  p40 (0,0,0)+(-1,0,0)
368  range [phi_4_0_min,phi_4_0_max] +(0,-1,0) +(0,0,1)
369 
370  universal: p40 (q41x_0,q41y_0,q41z_0)+(0.573580,0,0.81915)
371  p42 (0,0,0)+(0,1,0)
372  range [phi_4_1_min,phi_4_1_max] +(0,1,0)
373  [phi_4_2_min,phi_4_2_max] +(1,0,0)
374 
375  revolute: p42 (q43x_2,q43y_2,q43z_2)+(0,-1,0)
376  p43 (0,0,0)+(0,-1,0)
377  range [phi_4_3_min,phi_4_3_max] +(1,0,0) +(1,0,0)
378 
379  revolute: p43 (q44x_3,q44y_3,q44z_3)+(0,-1,0)
380  p44 (0,0,0)+(0,-1,0)
381  range [phi_4_4_min,phi_4_4_max] +(1,0,0) +(1,0,0)
382 
383 % KINEMATIC OF THE OBJECT-FINGER CONTACT
384 
385  SPHERICAL: p14 (q15x_4,q15y_4,q15z_4)
386  object (x1x_o_e,x1y_o_e,x1z_o_e)
387  range [beta_1] (q15x_4+m1x_4,q15y_4+m1y_4,q15z_4+m1z_4) (x1x_o,x1y_o,x1z_o)
388 
389  SPHERICAL: p24 (q25x_4,q25y_4,q25z_4)
390  object (x2x_o_e,x2y_o_e,x2z_o_e)
391  range [beta_2] (q25x_4+m2x_4,q25y_4+m2y_4,q25z_4+m2z_4) (x2x_o,x2y_o,x2z_o)
392 
393  SPHERICAL: p34 (q35x_4,q35y_4,q35z_4)
394  object (x3x_o_e,x3y_o_e,x3z_o_e)
395  range [beta_3] (q35x_4+m3x_4,q35y_4+m3y_4,q35z_4+m3z_4) (x3x_o,x3y_o,x3z_o)
396 
397  SPHERICAL: p44 (q45x_4,q45y_4,q45z_4)
398  object (x4x_o_e,x4y_o_e,x4z_o_e)
399  range [beta_4] (q45x_4+m4x_4,q45y_4+m4y_4,q45z_4+m4z_4) (x4x_o,x4y_o,x4z_o)
400 
401 
402