pr2_pitcher3.world
Go to the documentation of this file.
1 
2 [CONSTANTS]
3 
4  DEG2RAD:=PI/180
5 
6 [LINKS]
7 
8  world
9 
10  fridge_base: body "bodies/Fridge/friA-01.off" color (0.3,0.88,1)
11  body "bodies/Fridge/friA-02.off" color (0.3,0.88,1)
12  body "bodies/Fridge/friA-03.off" color (0.3,0.88,1)
13  body "bodies/Fridge/friA-04.off" color (0.3,0.88,1)
14  body "bodies/plane.off" yellow
15 
16  fridge_door: body "bodies/Fridge/friB-01.off" color (0.3,0.88,1)
17  body "bodies/Fridge/friB-02.off" color (0.3,0.88,1)
18  body "bodies/Fridge/friB-03.off" color (0.3,0.88,1)
19 
20  pitcher: body "bodies/pitcher.off" color (0.25,0.25,0)
21 
22  pitcher_virtual1
23  pitcher_virtual2
24  pitcher_virtual3
25 
26  base_footprint: body "bodies/base_link.off" white
27 
28  base: body "bodies/base_link.off" white
29 
30  laser_tilt: body "bodies/laser_tilt_mount_link.off" white
31 
32  wheel_rotation_1: body "bodies/bl_caster_rotation_link.off" white
33  wheel_1_left: body "bodies/bl_caster_l_wheel_link.off" black
34  wheel_1_right: body "bodies/bl_caster_r_wheel_link.off" black
35 
36  wheel_rotation_2: body "bodies/br_caster_rotation_link.off" white
37  wheel_2_left: body "bodies/br_caster_l_wheel_link.off" black
38  wheel_2_right: body "bodies/br_caster_r_wheel_link.off" black
39 
40  wheel_rotation_3: body "bodies/fl_caster_rotation_link.off" white
41  wheel_3_left: body "bodies/fl_caster_l_wheel_link.off" black
42  wheel_3_right: body "bodies/fl_caster_r_wheel_link.off" black
43 
44  wheel_rotation_4: body "bodies/fr_caster_rotation_link.off" white
45  wheel_4_left: body "bodies/fr_caster_l_wheel_link.off" black
46  wheel_4_right: body "bodies/fr_caster_r_wheel_link.off" black
47 
48  torso: body "bodies/torso_lift_link.off" white
49 
50  head_pan: body "bodies/head_pan_link.off" white
51  head_tilt: body "bodies/head_tilt_link.off" white
52 
53  l_shoulder_pan : body "bodies/l_shoulder_pan_link.off" white
54  l_shoulder_lift: body "bodies/l_shoulder_lift_link.off" white
55  l_upper_arm_roll: body "bodies/l_upper_arm_roll_link.off" white
56  l_upper_arm: body "bodies/l_upper_arm_link.off" white
57  l_elbow_flex: body "bodies/l_elbow_flex_link.off" white
58  l_forearm_roll: body "bodies/l_forearm_roll_link.off" white
59  l_forearm: body "bodies/l_forearm_link.off" white
60  l_wrist_flex: body "bodies/l_wrist_flex_link.off" white
61  l_wrist_roll: body "bodies/l_wrist_roll_link.off" white
62  l_gripper_palm: body "bodies/l_gripper_palm_link.off" white
63  l_gripper_l_finger: body "bodies/l_gripper_l_finger_link.off" white
64  l_gripper_l_finger_tip: body "bodies/l_gripper_l_finger_tip_link.off" white
65  l_gripper_r_finger: body "bodies/l_gripper_r_finger_link.off" white
66  l_gripper_r_finger_tip: body "bodies/l_gripper_r_finger_tip_link.off" white
67 
68 
69  r_shoulder_pan : body "bodies/r_shoulder_pan_link.off" white
70  r_shoulder_lift: body "bodies/r_shoulder_lift_link.off" white
71  r_upper_arm_roll: body "bodies/r_upper_arm_roll_link.off" white
72  r_upper_arm: body "bodies/r_upper_arm_link.off" white
73  r_elbow_flex: body "bodies/r_elbow_flex_link.off" white
74  r_forearm_roll: body "bodies/r_forearm_roll_link.off" white
75  r_forearm: body "bodies/r_forearm_link.off" white
76  r_wrist_flex: body "bodies/r_wrist_flex_link.off" white
77  r_wrist_roll: body "bodies/r_wrist_roll_link.off" white
78  r_gripper_palm: body "bodies/r_gripper_palm_link.off" white
79  r_gripper_l_finger: body "bodies/r_gripper_l_finger_link.off" white
80  r_gripper_l_finger_tip: body "bodies/r_gripper_l_finger_tip_link.off" white
81  r_gripper_r_finger: body "bodies/r_gripper_r_finger_link.off" white
82  r_gripper_r_finger_tip: body "bodies/r_gripper_r_finger_tip_link.off" white
83 
84 [JOINTS]
85 
86  fix: world fridge_base Rz(Pi/2)
87 
88  revolute: fridge_base (-0.3,-0.8,0)+(0,0,1)
89  fridge_door(-0.3,-0.8,0)+(0,0,1)
90  range [-0.06,2.3]
91  +(1,0,0)
92  +(1,0,0)
93 
94  fix: world base_footprint Id
95 
96  fix: base_footprint base
97  Txyz(0,0,0.051)
98 
99  fix: torso laser_tilt
100  Txyz (0.09893,0,0.227)
101 
102  fix: torso head_pan
103  Txyz(-0.01707,0,0.38145)
104 
105  fix: head_pan head_tilt
106  Txyz(0.068,0,0)
107 
108  /* Wheels */
109  fix: base wheel_rotation_1
110  Txyz (-0.2246,0.2246,0.0282)
111  fix: wheel_rotation_1 wheel_1_left
112  Txyz(0,0.049,0)
113  fix: wheel_rotation_1 wheel_1_right
114  Txyz(0,-0.049,0)
115 
116  fix: base wheel_rotation_2
117  Txyz (-0.2246,-0.2246,0.0282)
118  fix: wheel_rotation_2 wheel_2_left
119  Txyz(0,0.049,0)
120  fix: wheel_rotation_2 wheel_2_right
121  Txyz(0,-0.049,0)
122 
123  fix: base wheel_rotation_3
124  Txyz (0.2246,0.2246,0.0282)
125  fix: wheel_rotation_3 wheel_3_left
126  Txyz(0,0.049,0)
127  fix: wheel_rotation_3 wheel_3_right
128  Txyz(0,-0.049,0)
129 
130  fix: base wheel_rotation_4
131  Txyz (0.2246,-0.2246,0.0282)
132  fix: wheel_rotation_4 wheel_4_left
133  Txyz(0,0.049,0)
134  fix: wheel_rotation_4 wheel_4_right
135  Txyz(0,-0.049,0)
136 
137  /* Torso and arms (with grippers) */
138  prismatic: base (-0.05,0,0.739675)+(0,0,1)
139  torso (0,0,0)+(0,0,1)
140  range [0,0.31]
141 
142  revolute: torso (0,0.188,0)+(0,0,1)
143  l_shoulder_pan (0,0,0)+(0,0,1)
144  range [-40.9437*DEG2RAD,130.944*DEG2RAD]
145  +(1,0,0)
146  +(1,0,0)
147 
148  revolute: l_shoulder_pan (0.1,0,0)+(0,1,0)
149  l_shoulder_lift (0,0,0)+(0,1,0)
150  range [-30.0001*DEG2RAD,80.0021*DEG2RAD]
151  +(1,0,0)
152  +(1,0,0)
153 
154  revolute: l_shoulder_lift (0,0,0)+(1,0,0)
155  l_upper_arm_roll (0,0,0)+(1,0,0)
156  range [-45.8366*DEG2RAD,223.454*DEG2RAD]
157  +(0,1,0)
158  +(0,1,0)
159 
160  fix: l_upper_arm_roll l_upper_arm
161  Id
162 
163  revolute: l_upper_arm (0.4,0,0)+(0,1,0)
164  l_elbow_flex (0,0,0)+(0,1,0)
165  range [-133.001*DEG2RAD,0*DEG2RAD]
166  +(1,0,0)
167  +(1,0,0)
168 
169  revolute: l_elbow_flex (0,0,0)+(1,0,0)
170  l_forearm_roll (0,0,0)+(1,0,0)
171 
172  fix: l_forearm_roll l_forearm
173  Id
174 
175  revolute: l_forearm (0.321,0,0)+(0,1,0)
176  l_wrist_flex (0,0,0)+(0,1,0)
177  range [-124.905*DEG2RAD,0*DEG2RAD]
178  +(1,0,0)
179  +(1,0,0)
180 
181  revolute: l_wrist_flex (0,0,0)+(1,0,0)
182  l_wrist_roll (0,0,0)+(1,0,0)
183 
184  fix: l_wrist_roll l_gripper_palm
185  Id
186 
187  fix: l_gripper_palm l_gripper_l_finger
188  Txyz(0.07691,0.01,0)
189 
190  fix: l_gripper_l_finger l_gripper_l_finger_tip
191  Txyz(0.09137,0.00495,0)
192 
193  fix: l_gripper_palm l_gripper_r_finger
194  Txyz(0.07691,-0.01,0)*Rx(pi)
195 
196  fix: l_gripper_r_finger l_gripper_r_finger_tip
197  Txyz(0.09137,-0.00495,0)
198 
199  revolute: torso (0,-0.188,0)+(0,0,1)
200  r_shoulder_pan (0,0,0)+(0,0,1)
201  range [-130.944*DEG2RAD,40.9437*DEG2RAD]
202  +(1,0,0)
203  +(1,0,0)
204 
205  revolute: r_shoulder_pan (0.1,0,0)+(0,1,0)
206  r_shoulder_lift (0,0,0)+(0,1,0)
207  range [-30.0001*DEG2RAD,80.0021*DEG2RAD]
208  +(1,0,0)
209  +(1,0,0)
210 
211  revolute: r_shoulder_lift (0,0,0)+(1,0,0)
212  r_upper_arm_roll (0,0,0)+(1,0,0)
213  range [-223.454*DEG2RAD,45.8366*DEG2RAD]
214  +(0,1,0)
215  +(0,1,0)
216 
217  fix: r_upper_arm_roll r_upper_arm
218  Id
219 
220  revolute: r_upper_arm (0.4,0,0)+(0,1,0)
221  r_elbow_flex (0,0,0)+(0,1,0)
222  range [-133.001*DEG2RAD,0*DEG2RAD]
223  +(1,0,0)
224  +(1,0,0)
225 
226  revolute: r_elbow_flex (0,0,0)+(1,0,0)
227  r_forearm_roll (0,0,0)+(1,0,0)
228 
229  fix: r_forearm_roll r_forearm
230  Id
231 
232  revolute: r_forearm (0.321,0,0)+(0,1,0)
233  r_wrist_flex (0,0,0)+(0,1,0)
234  range [-124.905*DEG2RAD,0*DEG2RAD]
235  +(1,0,0)
236  +(1,0,0)
237 
238  revolute: r_wrist_flex (0,0,0)+(1,0,0)
239  r_wrist_roll (0,0,0)+(1,0,0)
240 
241  fix: r_wrist_roll r_gripper_palm
242  Id
243 
244  fix: r_gripper_palm r_gripper_l_finger
245  Txyz(0.07691,0.01,0)*Rz(pi/18)
246 
247  fix: r_gripper_l_finger r_gripper_l_finger_tip
248  Txyz(0.09137,0.00495,0)
249 
250  fix: r_gripper_palm r_gripper_r_finger
251  Txyz(0.07691,-0.01,0)*Rx(pi)*Rz(pi/18)
252 
253  fix: r_gripper_r_finger r_gripper_r_finger_tip
254  Txyz(0.09137,-0.00495,0)
255 
256  revolute: r_wrist_roll (0.18,0,0)+(0,0,1)
257  fridge_door(0.225,-0.748,0.93)+(0,0,1)
258 
259  fix: l_wrist_roll pitcher
260  Txyz(0.355,0.08,0)*Rz(Pi)
261 
262  prismatic: world (0,0,0)+(1,0,0)
263  pitcher_virtual1 (0,0,0)+(1,0,0)
264  range [0,1.5]
265 
266  prismatic: pitcher_virtual1 (0,0,0)+(0,1,0)
267  pitcher_virtual2 (0,0,0)+(0,1,0)
268  range [-0.5,1]
269 
270  prismatic: pitcher_virtual2 (0,0,0)+(0,0,1)
271  pitcher_virtual3 (0,0,0)+(0,0,1)
272  range [0.5,1.5]
273 
274  fix: pitcher_virtual3 pitcher
275  Rz(Pi)
276 
277 [COLLISIONS]
278 
279  do not check: all
280  check : fridge_door,pitcher
281  check : fridge_base, pitcher
282  check : r_gripper_palm, fridge_door
283  check : r_gripper_l_finger, fridge_door
284  check : r_gripper_r_finger, fridge_door
285  check : pitcher, torso
286  check : r_shoulder_pan, fridge_door
287  check : r_shoulder_lift, fridge_door
288  check : r_upper_arm_roll, fridge_door
289  check : r_upper_arm, fridge_door
290  check : r_elbow_flex, fridge_door
291  check : r_forearm_roll, fridge_door
292  check : r_forearm, fridge_door
293  check : r_wrist_flex, fridge_door
294  check : r_wrist_roll , fridge_door
295  check : l_shoulder_pan, fridge_door
296  check : l_shoulder_lift, fridge_door
297  check : l_upper_arm_roll, fridge_door
298  check : l_upper_arm, fridge_door
299  check : l_elbow_flex, fridge_door
300  check : l_forearm_roll, fridge_door
301  check : l_forearm, fridge_door
302  check : l_wrist_flex, fridge_door
303  check : l_wrist_roll , fridge_door
304 
305  check : l_shoulder_pan, r_shoulder_pan
306  check : l_shoulder_lift, r_shoulder_pan
307  check : l_upper_arm_roll, r_shoulder_pan
308  check : l_upper_arm, r_shoulder_pan
309  check : l_elbow_flex, r_shoulder_pan
310  check : l_forearm_roll, r_shoulder_pan
311  check : l_forearm, r_shoulder_pan
312  check : l_wrist_flex, r_shoulder_pan
313  check : l_wrist_roll, r_shoulder_pan
314 
315  check : l_shoulder_pan, r_upper_arm_roll
316  check : l_shoulder_lift, r_upper_arm_roll
317  check : l_upper_arm_roll, r_upper_arm_roll
318  check : l_upper_arm, r_upper_arm_roll
319  check : l_elbow_flex, r_upper_arm_roll
320  check : l_forearm_roll, r_upper_arm_roll
321  check : l_forearm, r_upper_arm_roll
322  check : l_wrist_flex, r_upper_arm_roll
323  check : l_wrist_roll, r_upper_arm_roll
324 
325  check : l_shoulder_pan, r_shoulder_lift
326  check : l_shoulder_lift, r_shoulder_lift
327  check : l_upper_arm_roll, r_shoulder_lift
328  check : l_upper_arm, r_shoulder_lift
329  check : l_elbow_flex, r_shoulder_lift
330  check : l_forearm_roll, r_shoulder_lift
331  check : l_forearm, r_shoulder_lift
332  check : l_wrist_flex, r_shoulder_lift
333  check : l_wrist_roll, r_shoulder_lift
334 
335  check : l_shoulder_pan, r_upper_arm
336  check : l_shoulder_lift, r_upper_arm
337  check : l_upper_arm_roll, r_upper_arm
338  check : l_upper_arm, r_upper_arm
339  check : l_elbow_flex, r_upper_arm
340  check : l_forearm_roll, r_upper_arm
341  check : l_forearm, r_upper_arm
342  check : l_wrist_flex, r_upper_arm
343  check : l_wrist_roll, r_upper_arm
344 
345  check : l_shoulder_pan, r_elbow_flex
346  check : l_shoulder_lift, r_elbow_flex
347  check : l_upper_arm_roll, r_elbow_flex
348  check : l_upper_arm, r_elbow_flex
349  check : l_elbow_flex, r_elbow_flex
350  check : l_forearm_roll, r_elbow_flex
351  check : l_forearm, r_elbow_flex
352  check : l_wrist_flex, r_elbow_flex
353  check : l_wrist_roll, r_elbow_flex
354 
355  check : l_shoulder_pan, r_forearm_roll
356  check : l_shoulder_lift, r_forearm_roll
357  check : l_upper_arm_roll, r_forearm_roll
358  check : l_upper_arm, r_forearm_roll
359  check : l_elbow_flex, r_forearm_roll
360  check : l_forearm_roll, r_forearm_roll
361  check : l_forearm, r_forearm_roll
362  check : l_wrist_flex, r_forearm_roll
363  check : l_wrist_roll, r_forearm_roll
364 
365  check : l_shoulder_pan, r_forearm
366  check : l_shoulder_lift, r_forearm
367  check : l_upper_arm_roll, r_forearm
368  check : l_upper_arm, r_forearm
369  check : l_elbow_flex, r_forearm
370  check : l_forearm_roll, r_forearm
371  check : l_forearm, r_forearm
372  check : l_wrist_flex, r_forearm
373  check : l_wrist_roll, r_forearm
374 
375  check : l_shoulder_pan, r_wrist_flex
376  check : l_shoulder_lift, r_wrist_flex
377  check : l_upper_arm_roll, r_wrist_flex
378  check : l_upper_arm, r_wrist_flex
379  check : l_elbow_flex, r_wrist_flex
380  check : l_forearm_roll, r_wrist_flex
381  check : l_forearm, r_wrist_flex
382  check : l_wrist_flex, r_wrist_flex
383  check : l_wrist_roll, r_wrist_flex
384 
385  check : l_shoulder_pan, r_wrist_roll
386  check : l_shoulder_lift, r_wrist_roll
387  check : l_upper_arm_roll, r_wrist_roll
388  check : l_upper_arm, r_wrist_roll
389  check : l_elbow_flex, r_wrist_roll
390  check : l_forearm_roll, r_wrist_roll
391  check : l_forearm, r_wrist_roll
392  check : l_wrist_flex, r_wrist_roll
393  check : l_wrist_roll, r_wrist_roll
394 
395  check : l_shoulder_pan, pitcher
396  check : l_shoulder_lift, pitcher
397  check : l_upper_arm_roll, pitcher
398  check : l_upper_arm, pitcher
399  check : l_elbow_flex, pitcher
400  check : l_forearm_roll, pitcher
401  check : l_forearm, pitcher
402  check : l_wrist_flex, pitcher
403  check : l_wrist_roll, pitcher
404 
405  check : l_shoulder_pan, fridge_base
406  check : l_shoulder_lift, fridge_base
407  check : l_upper_arm_roll, fridge_base
408  check : l_upper_arm, fridge_base
409  check : l_elbow_flex, fridge_base
410  check : l_forearm_roll, fridge_base
411  check : l_forearm, fridge_base
412  check : l_wrist_flex, fridge_base
413  check : l_wrist_roll, fridge_base
414  check : l_gripper_palm, fridge_base
415  check : l_gripper_l_finger, fridge_base
416  check : l_gripper_l_finger_tip, fridge_base
417  check : l_gripper_r_finger, fridge_base
418  check : l_gripper_r_finger_tip, fridge_base
419 
420  check : r_shoulder_pan, pitcher
421  check : r_shoulder_lift, pitcher
422  check : r_upper_arm_roll, pitcher
423  check : r_upper_arm, pitcher
424  check : r_elbow_flex, pitcher
425  check : r_forearm_roll, pitcher
426  check : r_forearm, pitcher
427  check : r_wrist_flex, pitcher
428  check : r_wrist_roll , pitcher
429  check : r_gripper_palm, pitcher
430  check : r_gripper_l_finger, pitcher
431  check : r_gripper_l_finger_tip, pitcher
432  check : r_gripper_r_finger, pitcher
433  check : r_gripper_r_finger_tip, pitcher
434 
435  check : head_pan, pitcher
436  check : head_tilt, pitcher
437 
#define DEG2RAD
Constant to transform from degrees to radiants.
Definition: defines.h:169