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.
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.
70 % Fingertip point vectors, qj5_4, and fingertip normal vector, mj_4
102 % Two ranges
for the universal joint connecting the palm and finger 1
109 % Range
for the first rotation in finger 1
113 % Range
for the second rotation in finger 1
117 % Range
for contact point in finger 1
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
124 phi_2_2_min:= phi_1_2_min
125 phi_2_2_max:= phi_1_2_max
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
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
135 % Range
for contact point in finger 2
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
142 phi_3_2_min:= phi_1_2_min
143 phi_3_2_max:= phi_1_2_max
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
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
153 % Range
for contact point in finger 3
156 % Two ranges
for the universal joint connecting the palm and finger 4
160 phi_4_1_min:= phi_1_1_min
161 phi_4_1_max:= phi_1_1_max
163 phi_4_2_min:= phi_1_2_min
164 phi_4_2_max:= phi_1_2_max
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
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
174 % Range
for contact point in finger 4
179 % Radius of the spheres representing contact points on the
object
182 % radius of the sphere modeling the fingertip
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 %--------------------------------------------------------------------
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
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
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
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
246 %-------------------------------------------------------------------
248 %--------------------------------------------------------------------
254 palm: body "bodies/palmSAH.off" white
255 body "bodies/acs1palm.off" blue
256 body "bodies/acs2palm.off" blue
257 body "bodies/acs3palm.off"
260 p12: body "bodies/p1.off" white
261 body "bodies/p2.off" white
262 body "bodies/b2.off" blue
264 p13: body "bodies/p3.off" white
266 p14: body "bodies/p4.off" white
267 body "bodies/b4.off" blue
270 p22: body "bodies/p1.off" white
271 body "bodies/p2.off" white
272 body "bodies/b2.off" blue
274 p23: body "bodies/p3.off" white
276 p24: body "bodies/p4.off" white
277 body "bodies/b4.off" blue
280 p32: body "bodies/p1.off" white
281 body "bodies/p2.off" white
282 body "bodies/b2.off" blue
284 p33: body "bodies/p3.off" white
286 p34: body "bodies/p4.off" white
287 body "bodies/b4.off" blue
290 p40: body "bodies/p0thumb.off" white
292 p42: body "bodies/p1.off" white
293 body "bodies/p2.off" white
294 body "bodies/b2.off" blue
296 p43: body "bodies/p3.off" white
298 p44: body "bodies/p4.off" white
299 body "bodies/b4.off" blue
302 %
object: one small circle at the contact points plus cylinders
303 % connecting the contact points (representing the
object body).
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)
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)
324 universal: palm (q11x,q11y,q11z)+(1,0,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)
329 revolute: p12 (q13x_2,q13y_2,q13z_2)+(0,-1,0)
331 range [phi_1_3_min,phi_1_3_max] +(1,0,0) +(1,0,0)
333 revolute: p13 (q14x_3,q14y_3,q14z_3)+(0,-1,0)
335 range [phi_1_4_min,phi_1_4_max] +(1,0,0) +(1,0,0)
338 universal: palm (q21x,q21y,q21z)+(1,0,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)
343 revolute: p22 (q23x_2,q23y_2,q23z_2)+(0,-1,0)
345 range [phi_2_3_min,phi_2_3_max] +(1,0,0) +(1,0,0)
347 revolute: p23 (q24x_3,q24y_3,q24z_3)+(0,-1,0)
349 range [phi_2_4_min,phi_2_4_max] +(1,0,0) +(1,0,0)
352 universal: palm (q31x,q31y,q31z)+(1,0,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)
357 revolute: p32 (q33x_2,q33y_2,q33z_2)+(0,-1,0)
359 range [phi_3_3_min,phi_3_3_max] +(1,0,0) +(1,0,0)
361 revolute: p33 (q34x_3,q34y_3,q34z_3)+(0,-1,0)
363 range [phi_3_4_min,phi_3_4_max] +(1,0,0) +(1,0,0)
366 revolute: palm (q40x,q40y,q40z)+(0,0,-1)
368 range [phi_4_0_min,phi_4_0_max] +(0,-1,0) +(0,0,1)
370 universal: p40 (q41x_0,q41y_0,q41z_0)+(0.573580,0,0.81915)
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)
375 revolute: p42 (q43x_2,q43y_2,q43z_2)+(0,-1,0)
377 range [phi_4_3_min,phi_4_3_max] +(1,0,0) +(1,0,0)
379 revolute: p43 (q44x_3,q44y_3,q44z_3)+(0,-1,0)
381 range [phi_4_4_min,phi_4_4_max] +(1,0,0) +(1,0,0)
383 % KINEMATIC OF THE OBJECT-FINGER CONTACT
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)
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)
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)
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)
Follow us!