Merge lp:~guihome/sr-manipulation/manip-electric_corr3 into lp:sr-manipulation
- manip-electric_corr3
- Merge into trunk
Proposed by
Guillaume W.
Status: | Merged |
---|---|
Merged at revision: | 316 |
Proposed branch: | lp:~guihome/sr-manipulation/manip-electric_corr3 |
Merge into: | lp:sr-manipulation |
Diff against target: |
4232 lines (+2424/-1366) 23 files modified
shadow_arm_navigation/config/joint_limits.yaml (+12/-12) shadow_arm_navigation/config/ompl_planning.yaml (+9/-9) shadow_arm_navigation/config/shadow_planning_description.yaml (+1092/-1253) shadow_arm_navigation/launch/constraint_aware_kinematics.launch (+1/-1) shadow_arm_navigation/launch/shadow_arm_navigation.launch (+1/-1) sr_gui_manipulation/lib/SrGuiManipulation.py (+177/-63) sr_gui_manipulation/manifest.xml (+1/-0) sr_object_manipulation_launch/config/gazebo_tactile_thresholds.yaml (+10/-10) sr_object_manipulation_launch/config/hand_description.yaml (+1/-1) sr_object_manipulation_launch/config/household_object_server.yaml (+2/-2) sr_object_manipulation_launch/config/self_filter.yaml (+1/-1) sr_object_manipulation_launch/launch/arm_and_hand_motor.launch (+1/-1) sr_object_manipulation_launch/launch/collision_map.launch (+1/-1) sr_object_manipulation_launch/launch/kinect_static_transform.launch (+4/-2) sr_object_manipulation_launch/launch/manipulator.launch (+4/-4) sr_object_manipulation_launch/launch/static_transform.launch (+2/-4) sr_object_manipulation_launch/launch/tabletop_node.launch (+1/-1) sr_pick_and_place/CMakeLists.txt (+30/-0) sr_pick_and_place/Makefile (+1/-0) sr_pick_and_place/mainpage.dox (+14/-0) sr_pick_and_place/manifest.xml (+21/-0) sr_pick_and_place/src/execution.py (+601/-0) sr_pick_and_place/src/planification.py (+437/-0) |
To merge this branch: | bzr merge lp:~guihome/sr-manipulation/manip-electric_corr3 |
Related bugs: |
Reviewer | Review Type | Date Requested | Status |
---|---|---|---|
Shadow Robot | Pending | ||
Review via email: mp+132253@code.launchpad.net |
Commit message
Description of the change
Mainly added sr_pick_and_place
and cleaned some unused stuff (frames, platform, collision pairs etc...) + updated some others for this new object_
To post a comment you must log in.
Revision history for this message
Ugo (ugocupcic) wrote : | # |
Preview Diff
[H/L] Next/Prev Comment, [J/K] Next/Prev File, [N/P] Next/Prev Hunk
1 | === modified file 'shadow_arm_navigation/config/joint_limits.yaml' |
2 | --- shadow_arm_navigation/config/joint_limits.yaml 2012-07-27 20:16:07 +0000 |
3 | +++ shadow_arm_navigation/config/joint_limits.yaml 2012-10-31 08:29:25 +0000 |
4 | @@ -2,42 +2,42 @@ |
5 | ShoulderJRotate: |
6 | has_position_limits: true |
7 | has_velocity_limits: true |
8 | - max_velocity: 0.5 |
9 | + max_velocity: 0.1 |
10 | has_acceleration_limits: true |
11 | - max_acceleration: 1 |
12 | + max_acceleration: 0.5 |
13 | angle_wraparound: false |
14 | ShoulderJSwing: |
15 | has_position_limits: true |
16 | has_velocity_limits: true |
17 | - max_velocity: 0.5 |
18 | + max_velocity: 0.1 |
19 | has_acceleration_limits: true |
20 | - max_acceleration: 1 |
21 | + max_acceleration: 0.5 |
22 | angle_wraparound: false |
23 | ElbowJSwing: |
24 | has_position_limits: true |
25 | has_velocity_limits: true |
26 | - max_velocity: 0.5 |
27 | + max_velocity: 0.1 |
28 | has_acceleration_limits: true |
29 | - max_acceleration: 1 |
30 | + max_acceleration: 0.5 |
31 | angle_wraparound: false |
32 | ElbowJRotate: |
33 | has_position_limits: true |
34 | has_velocity_limits: true |
35 | - max_velocity: 0.5 |
36 | + max_velocity: 0.1 |
37 | has_acceleration_limits: true |
38 | - max_acceleration: 1 |
39 | + max_acceleration: 0.5 |
40 | angle_wraparound: false |
41 | WRJ2: |
42 | has_position_limits: true |
43 | has_velocity_limits: true |
44 | - max_velocity: 1 |
45 | + max_velocity: 0.1 |
46 | has_acceleration_limits: true |
47 | - max_acceleration: 1 |
48 | + max_acceleration: 0.5 |
49 | angle_wraparound: false |
50 | WRJ1: |
51 | has_position_limits: true |
52 | has_velocity_limits: true |
53 | - max_velocity: 1 |
54 | + max_velocity: 0.1 |
55 | has_acceleration_limits: true |
56 | - max_acceleration: 1 |
57 | + max_acceleration: 0.5 |
58 | angle_wraparound: false |
59 | |
60 | === modified file 'shadow_arm_navigation/config/ompl_planning.yaml' |
61 | --- shadow_arm_navigation/config/ompl_planning.yaml 2012-02-28 09:17:59 +0000 |
62 | +++ shadow_arm_navigation/config/ompl_planning.yaml 2012-10-31 08:29:25 +0000 |
63 | @@ -13,7 +13,7 @@ |
64 | - LBKPIECEkConfig1 |
65 | projection_evaluator: joint_state |
66 | right_arm_cartesian: |
67 | - parent_frame: desk_support |
68 | + parent_frame: shadowarm_base |
69 | physical_group: right_arm |
70 | planner_type: RPYIKTaskSpacePlanner |
71 | state_spaces: |
72 | @@ -25,16 +25,16 @@ |
73 | - yaw |
74 | x: |
75 | type: Linear |
76 | - min: -2.0 |
77 | - max: 2.0 |
78 | + min: 0.0 |
79 | + max: 1.0 |
80 | y: |
81 | type: Linear |
82 | - min: -2.0 |
83 | - max: 2.0 |
84 | + min: -0.7 |
85 | + max: 0.7 |
86 | z: |
87 | type: Linear |
88 | - min: -2.0 |
89 | - max: 2.0 |
90 | + min: -0.2 |
91 | + max: 1.0 |
92 | roll: |
93 | type: Revolute |
94 | pitch: |
95 | @@ -46,6 +46,6 @@ |
96 | - LBKPIECEkConfig1 |
97 | kinematics_solver: arm_kinematics_constraint_aware/KDLArmKinematicsPlugin |
98 | tip_name: palm |
99 | - root_name: desk_support |
100 | + root_name: shadowarm_base |
101 | projection_evaluator: joint_state |
102 | - longest_valid_segment_fraction: 0.001 |
103 | \ No newline at end of file |
104 | + longest_valid_segment_fraction: 0.001 |
105 | |
106 | === modified file 'shadow_arm_navigation/config/shadow_planning_description.yaml' |
107 | --- shadow_arm_navigation/config/shadow_planning_description.yaml 2012-07-25 20:54:34 +0000 |
108 | +++ shadow_arm_navigation/config/shadow_planning_description.yaml 2012-10-31 08:29:25 +0000 |
109 | @@ -1,22 +1,13 @@ |
110 | multi_dof_joints: |
111 | - name: world_joint |
112 | type: Floating |
113 | - parent_frame_id: desk_support |
114 | - child_frame_id: desk_support |
115 | + parent_frame_id: world |
116 | + child_frame_id: world |
117 | groups: |
118 | - name: right_arm |
119 | - base_link: desk_support |
120 | + base_link: shadowarm_base |
121 | tip_link: palm |
122 | default_collision_operations: |
123 | - - object1: world |
124 | - object2: desk_support |
125 | - operation: disable # Adjacent in collision |
126 | - - object1: desk_support |
127 | - object2: desk_pillar |
128 | - operation: disable # Adjacent in collision |
129 | - - object1: desk_pillar |
130 | - object2: shadowarm_base |
131 | - operation: disable # Adjacent in collision |
132 | - object1: shadowarm_base |
133 | object2: shadowarm_trunk |
134 | operation: disable # Adjacent in collision |
135 | @@ -27,9 +18,9 @@ |
136 | object2: shadowarm_lowerarm |
137 | operation: disable # Adjacent in collision |
138 | - object1: shadowarm_lowerarm |
139 | - object2: shadow_handsupport |
140 | + object2: shadowarm_handsupport |
141 | operation: disable # Adjacent in collision |
142 | - - object1: shadow_handsupport |
143 | + - object1: shadowarm_handsupport |
144 | object2: forearm |
145 | operation: disable # Adjacent in collision |
146 | - object1: forearm |
147 | @@ -119,1249 +110,1097 @@ |
148 | - object1: camera_rgb_frame |
149 | object2: camera_rgb_optical_frame |
150 | operation: disable # Adjacent in collision |
151 | - - object1: desk_pillar |
152 | - object2: thproximal |
153 | - operation: disable # Default in collision |
154 | - object1: forearm |
155 | object2: shadowarm_lowerarm |
156 | operation: disable # Often in collision |
157 | - object1: camera_link |
158 | - object2: desk_pillar |
159 | - operation: disable # Never in collision |
160 | - - object1: camera_link |
161 | - object2: desk_support |
162 | - operation: disable # Never in collision |
163 | - - object1: camera_link |
164 | - object2: ffdistal |
165 | - operation: disable # Never in collision |
166 | - - object1: camera_link |
167 | - object2: ffknuckle |
168 | - operation: disable # Never in collision |
169 | - - object1: camera_link |
170 | - object2: ffmiddle |
171 | - operation: disable # Never in collision |
172 | - - object1: camera_link |
173 | - object2: ffproximal |
174 | - operation: disable # Never in collision |
175 | - - object1: camera_link |
176 | - object2: forearm |
177 | - operation: disable # Never in collision |
178 | - - object1: camera_link |
179 | - object2: lfdistal |
180 | - operation: disable # Never in collision |
181 | - - object1: camera_link |
182 | - object2: lfknuckle |
183 | - operation: disable # Never in collision |
184 | - - object1: camera_link |
185 | - object2: lfmetacarpal |
186 | - operation: disable # Never in collision |
187 | - - object1: camera_link |
188 | - object2: lfmiddle |
189 | - operation: disable # Never in collision |
190 | - - object1: camera_link |
191 | - object2: lfproximal |
192 | - operation: disable # Never in collision |
193 | - - object1: camera_link |
194 | - object2: mfdistal |
195 | - operation: disable # Never in collision |
196 | - - object1: camera_link |
197 | - object2: mfknuckle |
198 | - operation: disable # Never in collision |
199 | - - object1: camera_link |
200 | - object2: mfmiddle |
201 | - operation: disable # Never in collision |
202 | - - object1: camera_link |
203 | - object2: mfproximal |
204 | - operation: disable # Never in collision |
205 | - - object1: camera_link |
206 | - object2: palm |
207 | - operation: disable # Never in collision |
208 | - - object1: camera_link |
209 | - object2: rfdistal |
210 | - operation: disable # Never in collision |
211 | - - object1: camera_link |
212 | - object2: rfknuckle |
213 | - operation: disable # Never in collision |
214 | - - object1: camera_link |
215 | - object2: rfmiddle |
216 | - operation: disable # Never in collision |
217 | - - object1: camera_link |
218 | - object2: rfproximal |
219 | - operation: disable # Never in collision |
220 | - - object1: camera_link |
221 | - object2: shadowarm_base |
222 | - operation: disable # Never in collision |
223 | - - object1: camera_link |
224 | - object2: shadow_handsupport |
225 | - operation: disable # Never in collision |
226 | - - object1: camera_link |
227 | - object2: shadowarm_lowerarm |
228 | - operation: disable # Never in collision |
229 | - - object1: camera_link |
230 | - object2: shadowarm_upperarm |
231 | - operation: disable # Never in collision |
232 | - - object1: camera_link |
233 | - object2: thbase |
234 | - operation: disable # Never in collision |
235 | - - object1: camera_link |
236 | - object2: thdistal |
237 | - operation: disable # Never in collision |
238 | - - object1: camera_link |
239 | - object2: thhub |
240 | - operation: disable # Never in collision |
241 | - - object1: camera_link |
242 | - object2: thmiddle |
243 | - operation: disable # Never in collision |
244 | - - object1: camera_link |
245 | - object2: thproximal |
246 | - operation: disable # Never in collision |
247 | - - object1: camera_link |
248 | - object2: wrist |
249 | - operation: disable # Never in collision |
250 | - - object1: desk_pillar |
251 | - object2: ffmiddle |
252 | - operation: disable # Never in collision |
253 | - - object1: desk_pillar |
254 | - object2: forearm |
255 | - operation: disable # Never in collision |
256 | - - object1: desk_pillar |
257 | - object2: lfknuckle |
258 | - operation: disable # Never in collision |
259 | - - object1: desk_pillar |
260 | - object2: lfmetacarpal |
261 | - operation: disable # Never in collision |
262 | - - object1: desk_pillar |
263 | - object2: lfproximal |
264 | - operation: disable # Never in collision |
265 | - - object1: desk_pillar |
266 | - object2: mfknuckle |
267 | - operation: disable # Never in collision |
268 | - - object1: desk_pillar |
269 | - object2: mfmiddle |
270 | - operation: disable # Never in collision |
271 | - - object1: desk_pillar |
272 | - object2: mfproximal |
273 | - operation: disable # Never in collision |
274 | - - object1: desk_pillar |
275 | - object2: palm |
276 | - operation: disable # Never in collision |
277 | - - object1: desk_pillar |
278 | - object2: rfknuckle |
279 | - operation: disable # Never in collision |
280 | - - object1: desk_pillar |
281 | - object2: rfproximal |
282 | - operation: disable # Never in collision |
283 | - - object1: desk_pillar |
284 | - object2: shadow_handsupport |
285 | - operation: disable # Never in collision |
286 | - - object1: desk_pillar |
287 | - object2: shadowarm_lowerarm |
288 | - operation: disable # Never in collision |
289 | - - object1: desk_pillar |
290 | - object2: shadowarm_trunk |
291 | - operation: disable # Never in collision |
292 | - - object1: desk_pillar |
293 | - object2: shadowarm_upperarm |
294 | - operation: disable # Never in collision |
295 | - - object1: desk_pillar |
296 | - object2: thbase |
297 | - operation: disable # Never in collision |
298 | - - object1: desk_pillar |
299 | - object2: thhub |
300 | - operation: disable # Never in collision |
301 | - - object1: desk_pillar |
302 | - object2: wrist |
303 | - operation: disable # Never in collision |
304 | - - object1: desk_support |
305 | - object2: ffdistal |
306 | - operation: disable # Never in collision |
307 | - - object1: desk_support |
308 | - object2: ffknuckle |
309 | - operation: disable # Never in collision |
310 | - - object1: desk_support |
311 | - object2: ffmiddle |
312 | - operation: disable # Never in collision |
313 | - - object1: desk_support |
314 | - object2: ffproximal |
315 | - operation: disable # Never in collision |
316 | - - object1: desk_support |
317 | - object2: forearm |
318 | - operation: disable # Never in collision |
319 | - - object1: desk_support |
320 | - object2: lfdistal |
321 | - operation: disable # Never in collision |
322 | - - object1: desk_support |
323 | - object2: lfknuckle |
324 | - operation: disable # Never in collision |
325 | - - object1: desk_support |
326 | - object2: lfmetacarpal |
327 | - operation: disable # Never in collision |
328 | - - object1: desk_support |
329 | - object2: lfmiddle |
330 | - operation: disable # Never in collision |
331 | - - object1: desk_support |
332 | - object2: lfproximal |
333 | - operation: disable # Never in collision |
334 | - - object1: desk_support |
335 | - object2: mfdistal |
336 | - operation: disable # Never in collision |
337 | - - object1: desk_support |
338 | - object2: mfknuckle |
339 | - operation: disable # Never in collision |
340 | - - object1: desk_support |
341 | - object2: mfmiddle |
342 | - operation: disable # Never in collision |
343 | - - object1: desk_support |
344 | - object2: mfproximal |
345 | - operation: disable # Never in collision |
346 | - - object1: desk_support |
347 | - object2: palm |
348 | - operation: disable # Never in collision |
349 | - - object1: desk_support |
350 | - object2: rfdistal |
351 | - operation: disable # Never in collision |
352 | - - object1: desk_support |
353 | - object2: rfknuckle |
354 | - operation: disable # Never in collision |
355 | - - object1: desk_support |
356 | - object2: rfmiddle |
357 | - operation: disable # Never in collision |
358 | - - object1: desk_support |
359 | - object2: rfproximal |
360 | - operation: disable # Never in collision |
361 | - - object1: desk_support |
362 | - object2: shadowarm_base |
363 | - operation: disable # Never in collision |
364 | - - object1: desk_support |
365 | - object2: shadow_handsupport |
366 | - operation: disable # Never in collision |
367 | - - object1: desk_support |
368 | - object2: shadowarm_lowerarm |
369 | - operation: disable # Never in collision |
370 | - - object1: desk_support |
371 | - object2: shadowarm_trunk |
372 | - operation: disable # Never in collision |
373 | - - object1: desk_support |
374 | - object2: shadowarm_upperarm |
375 | - operation: disable # Never in collision |
376 | - - object1: desk_support |
377 | - object2: thbase |
378 | - operation: disable # Never in collision |
379 | - - object1: desk_support |
380 | - object2: thdistal |
381 | - operation: disable # Never in collision |
382 | - - object1: desk_support |
383 | - object2: thhub |
384 | - operation: disable # Never in collision |
385 | - - object1: desk_support |
386 | - object2: thmiddle |
387 | - operation: disable # Never in collision |
388 | - - object1: desk_support |
389 | - object2: thproximal |
390 | - operation: disable # Never in collision |
391 | - - object1: desk_support |
392 | - object2: wrist |
393 | - operation: disable # Never in collision |
394 | - - object1: ffdistal |
395 | - object2: ffknuckle |
396 | - operation: disable # Never in collision |
397 | - - object1: ffdistal |
398 | - object2: ffproximal |
399 | - operation: disable # Never in collision |
400 | - - object1: ffdistal |
401 | - object2: forearm |
402 | - operation: disable # Never in collision |
403 | - - object1: ffdistal |
404 | - object2: lfknuckle |
405 | - operation: disable # Never in collision |
406 | - - object1: ffdistal |
407 | - object2: lfmetacarpal |
408 | - operation: disable # Never in collision |
409 | - - object1: ffdistal |
410 | - object2: lfmiddle |
411 | - operation: disable # Never in collision |
412 | - - object1: ffdistal |
413 | - object2: lfproximal |
414 | - operation: disable # Never in collision |
415 | - - object1: ffdistal |
416 | - object2: mfknuckle |
417 | - operation: disable # Never in collision |
418 | - - object1: ffdistal |
419 | - object2: mfproximal |
420 | - operation: disable # Never in collision |
421 | - - object1: ffdistal |
422 | - object2: palm |
423 | - operation: disable # Never in collision |
424 | - - object1: ffdistal |
425 | - object2: rfknuckle |
426 | - operation: disable # Never in collision |
427 | - - object1: ffdistal |
428 | - object2: rfmiddle |
429 | - operation: disable # Never in collision |
430 | - - object1: ffdistal |
431 | - object2: rfproximal |
432 | - operation: disable # Never in collision |
433 | - - object1: ffdistal |
434 | - object2: shadowarm_base |
435 | - operation: disable # Never in collision |
436 | - - object1: ffdistal |
437 | - object2: shadow_handsupport |
438 | - operation: disable # Never in collision |
439 | - - object1: ffdistal |
440 | - object2: shadowarm_lowerarm |
441 | - operation: disable # Never in collision |
442 | - - object1: ffdistal |
443 | - object2: shadowarm_trunk |
444 | - operation: disable # Never in collision |
445 | - - object1: ffdistal |
446 | - object2: shadowarm_upperarm |
447 | - operation: disable # Never in collision |
448 | - - object1: ffdistal |
449 | - object2: thbase |
450 | - operation: disable # Never in collision |
451 | - - object1: ffdistal |
452 | - object2: thhub |
453 | - operation: disable # Never in collision |
454 | - - object1: ffdistal |
455 | - object2: thproximal |
456 | - operation: disable # Never in collision |
457 | - - object1: ffdistal |
458 | - object2: wrist |
459 | - operation: disable # Never in collision |
460 | - - object1: ffknuckle |
461 | - object2: ffmiddle |
462 | - operation: disable # Never in collision |
463 | - - object1: ffknuckle |
464 | - object2: forearm |
465 | - operation: disable # Never in collision |
466 | - - object1: ffknuckle |
467 | - object2: lfdistal |
468 | - operation: disable # Never in collision |
469 | - - object1: ffknuckle |
470 | - object2: lfknuckle |
471 | - operation: disable # Never in collision |
472 | - - object1: ffknuckle |
473 | - object2: lfmetacarpal |
474 | - operation: disable # Never in collision |
475 | - - object1: ffknuckle |
476 | - object2: lfmiddle |
477 | - operation: disable # Never in collision |
478 | - - object1: ffknuckle |
479 | - object2: lfproximal |
480 | - operation: disable # Never in collision |
481 | - - object1: ffknuckle |
482 | - object2: mfdistal |
483 | - operation: disable # Never in collision |
484 | - - object1: ffknuckle |
485 | - object2: mfknuckle |
486 | - operation: disable # Never in collision |
487 | - - object1: ffknuckle |
488 | - object2: mfmiddle |
489 | - operation: disable # Never in collision |
490 | - - object1: ffknuckle |
491 | - object2: mfproximal |
492 | - operation: disable # Never in collision |
493 | - - object1: ffknuckle |
494 | - object2: rfdistal |
495 | - operation: disable # Never in collision |
496 | - - object1: ffknuckle |
497 | - object2: rfknuckle |
498 | - operation: disable # Never in collision |
499 | - - object1: ffknuckle |
500 | - object2: rfmiddle |
501 | - operation: disable # Never in collision |
502 | - - object1: ffknuckle |
503 | - object2: rfproximal |
504 | - operation: disable # Never in collision |
505 | - - object1: ffknuckle |
506 | - object2: shadowarm_base |
507 | - operation: disable # Never in collision |
508 | - - object1: ffknuckle |
509 | - object2: shadow_handsupport |
510 | - operation: disable # Never in collision |
511 | - - object1: ffknuckle |
512 | - object2: shadowarm_lowerarm |
513 | - operation: disable # Never in collision |
514 | - - object1: ffknuckle |
515 | - object2: shadowarm_trunk |
516 | - operation: disable # Never in collision |
517 | - - object1: ffknuckle |
518 | - object2: shadowarm_upperarm |
519 | - operation: disable # Never in collision |
520 | - - object1: ffknuckle |
521 | - object2: thbase |
522 | - operation: disable # Never in collision |
523 | - - object1: ffknuckle |
524 | - object2: thhub |
525 | - operation: disable # Never in collision |
526 | - - object1: ffknuckle |
527 | - object2: thmiddle |
528 | - operation: disable # Never in collision |
529 | - - object1: ffknuckle |
530 | - object2: thproximal |
531 | - operation: disable # Never in collision |
532 | - - object1: ffknuckle |
533 | - object2: wrist |
534 | - operation: disable # Never in collision |
535 | - - object1: ffmiddle |
536 | - object2: forearm |
537 | - operation: disable # Never in collision |
538 | - - object1: ffmiddle |
539 | - object2: lfdistal |
540 | - operation: disable # Never in collision |
541 | - - object1: ffmiddle |
542 | - object2: lfknuckle |
543 | - operation: disable # Never in collision |
544 | - - object1: ffmiddle |
545 | - object2: lfmetacarpal |
546 | - operation: disable # Never in collision |
547 | - - object1: ffmiddle |
548 | - object2: lfmiddle |
549 | - operation: disable # Never in collision |
550 | - - object1: ffmiddle |
551 | - object2: lfproximal |
552 | - operation: disable # Never in collision |
553 | - - object1: ffmiddle |
554 | - object2: mfknuckle |
555 | - operation: disable # Never in collision |
556 | - - object1: ffmiddle |
557 | - object2: palm |
558 | - operation: disable # Never in collision |
559 | - - object1: ffmiddle |
560 | - object2: rfdistal |
561 | - operation: disable # Never in collision |
562 | - - object1: ffmiddle |
563 | - object2: rfknuckle |
564 | - operation: disable # Never in collision |
565 | - - object1: ffmiddle |
566 | - object2: rfmiddle |
567 | - operation: disable # Never in collision |
568 | - - object1: ffmiddle |
569 | - object2: rfproximal |
570 | - operation: disable # Never in collision |
571 | - - object1: ffmiddle |
572 | - object2: shadowarm_base |
573 | - operation: disable # Never in collision |
574 | - - object1: ffmiddle |
575 | - object2: shadow_handsupport |
576 | - operation: disable # Never in collision |
577 | - - object1: ffmiddle |
578 | - object2: shadowarm_lowerarm |
579 | - operation: disable # Never in collision |
580 | - - object1: ffmiddle |
581 | - object2: shadowarm_trunk |
582 | - operation: disable # Never in collision |
583 | - - object1: ffmiddle |
584 | - object2: shadowarm_upperarm |
585 | - operation: disable # Never in collision |
586 | - - object1: ffmiddle |
587 | - object2: thbase |
588 | - operation: disable # Never in collision |
589 | - - object1: ffmiddle |
590 | - object2: thhub |
591 | - operation: disable # Never in collision |
592 | - - object1: ffmiddle |
593 | - object2: thmiddle |
594 | - operation: disable # Never in collision |
595 | - - object1: ffmiddle |
596 | - object2: thproximal |
597 | - operation: disable # Never in collision |
598 | - - object1: ffmiddle |
599 | - object2: wrist |
600 | - operation: disable # Never in collision |
601 | - - object1: ffproximal |
602 | - object2: forearm |
603 | - operation: disable # Never in collision |
604 | - - object1: ffproximal |
605 | - object2: lfdistal |
606 | - operation: disable # Never in collision |
607 | - - object1: ffproximal |
608 | - object2: lfknuckle |
609 | - operation: disable # Never in collision |
610 | - - object1: ffproximal |
611 | - object2: lfmetacarpal |
612 | - operation: disable # Never in collision |
613 | - - object1: ffproximal |
614 | - object2: lfmiddle |
615 | - operation: disable # Never in collision |
616 | - - object1: ffproximal |
617 | - object2: lfproximal |
618 | - operation: disable # Never in collision |
619 | - - object1: ffproximal |
620 | - object2: mfdistal |
621 | - operation: disable # Never in collision |
622 | - - object1: ffproximal |
623 | - object2: mfknuckle |
624 | - operation: disable # Never in collision |
625 | - - object1: ffproximal |
626 | - object2: mfmiddle |
627 | - operation: disable # Never in collision |
628 | - - object1: ffproximal |
629 | - object2: palm |
630 | - operation: disable # Never in collision |
631 | - - object1: ffproximal |
632 | - object2: rfdistal |
633 | - operation: disable # Never in collision |
634 | - - object1: ffproximal |
635 | - object2: rfknuckle |
636 | - operation: disable # Never in collision |
637 | - - object1: ffproximal |
638 | - object2: rfmiddle |
639 | - operation: disable # Never in collision |
640 | - - object1: ffproximal |
641 | - object2: rfproximal |
642 | - operation: disable # Never in collision |
643 | - - object1: ffproximal |
644 | - object2: shadowarm_base |
645 | - operation: disable # Never in collision |
646 | - - object1: ffproximal |
647 | - object2: shadow_handsupport |
648 | - operation: disable # Never in collision |
649 | - - object1: ffproximal |
650 | - object2: shadowarm_lowerarm |
651 | - operation: disable # Never in collision |
652 | - - object1: ffproximal |
653 | - object2: shadowarm_trunk |
654 | - operation: disable # Never in collision |
655 | - - object1: ffproximal |
656 | - object2: shadowarm_upperarm |
657 | - operation: disable # Never in collision |
658 | - - object1: ffproximal |
659 | - object2: thbase |
660 | - operation: disable # Never in collision |
661 | - - object1: ffproximal |
662 | - object2: thhub |
663 | - operation: disable # Never in collision |
664 | - - object1: ffproximal |
665 | - object2: thmiddle |
666 | - operation: disable # Never in collision |
667 | - - object1: ffproximal |
668 | - object2: thproximal |
669 | - operation: disable # Never in collision |
670 | - - object1: ffproximal |
671 | - object2: wrist |
672 | - operation: disable # Never in collision |
673 | - - object1: forearm |
674 | - object2: lfdistal |
675 | - operation: disable # Never in collision |
676 | - - object1: forearm |
677 | - object2: lfknuckle |
678 | - operation: disable # Never in collision |
679 | - - object1: forearm |
680 | - object2: lfmetacarpal |
681 | - operation: disable # Never in collision |
682 | - - object1: forearm |
683 | - object2: lfmiddle |
684 | - operation: disable # Never in collision |
685 | - - object1: forearm |
686 | - object2: lfproximal |
687 | - operation: disable # Never in collision |
688 | - - object1: forearm |
689 | - object2: mfdistal |
690 | - operation: disable # Never in collision |
691 | - - object1: forearm |
692 | - object2: mfknuckle |
693 | - operation: disable # Never in collision |
694 | - - object1: forearm |
695 | - object2: mfmiddle |
696 | - operation: disable # Never in collision |
697 | - - object1: forearm |
698 | - object2: mfproximal |
699 | - operation: disable # Never in collision |
700 | - - object1: forearm |
701 | - object2: palm |
702 | - operation: disable # Never in collision |
703 | - - object1: forearm |
704 | - object2: rfdistal |
705 | - operation: disable # Never in collision |
706 | - - object1: forearm |
707 | - object2: rfknuckle |
708 | - operation: disable # Never in collision |
709 | - - object1: forearm |
710 | - object2: rfmiddle |
711 | - operation: disable # Never in collision |
712 | - - object1: forearm |
713 | - object2: rfproximal |
714 | - operation: disable # Never in collision |
715 | - - object1: forearm |
716 | - object2: shadowarm_base |
717 | - operation: disable # Never in collision |
718 | - - object1: forearm |
719 | - object2: shadowarm_trunk |
720 | - operation: disable # Never in collision |
721 | - - object1: forearm |
722 | - object2: shadowarm_upperarm |
723 | - operation: disable # Never in collision |
724 | - - object1: forearm |
725 | - object2: thbase |
726 | - operation: disable # Never in collision |
727 | - - object1: forearm |
728 | - object2: thdistal |
729 | - operation: disable # Never in collision |
730 | - - object1: forearm |
731 | - object2: thhub |
732 | - operation: disable # Never in collision |
733 | - - object1: forearm |
734 | - object2: thmiddle |
735 | - operation: disable # Never in collision |
736 | - - object1: forearm |
737 | - object2: thproximal |
738 | - operation: disable # Never in collision |
739 | - - object1: lfdistal |
740 | - object2: lfknuckle |
741 | - operation: disable # Never in collision |
742 | - - object1: lfdistal |
743 | - object2: lfmetacarpal |
744 | - operation: disable # Never in collision |
745 | - - object1: lfdistal |
746 | - object2: lfproximal |
747 | - operation: disable # Never in collision |
748 | - - object1: lfdistal |
749 | - object2: mfknuckle |
750 | - operation: disable # Never in collision |
751 | - - object1: lfdistal |
752 | - object2: mfproximal |
753 | - operation: disable # Never in collision |
754 | - - object1: lfdistal |
755 | - object2: palm |
756 | - operation: disable # Never in collision |
757 | - - object1: lfdistal |
758 | - object2: rfknuckle |
759 | - operation: disable # Never in collision |
760 | - - object1: lfdistal |
761 | - object2: shadowarm_base |
762 | - operation: disable # Never in collision |
763 | - - object1: lfdistal |
764 | - object2: shadow_handsupport |
765 | - operation: disable # Never in collision |
766 | - - object1: lfdistal |
767 | - object2: shadowarm_lowerarm |
768 | - operation: disable # Never in collision |
769 | - - object1: lfdistal |
770 | - object2: shadowarm_trunk |
771 | - operation: disable # Never in collision |
772 | - - object1: lfdistal |
773 | - object2: shadowarm_upperarm |
774 | - operation: disable # Never in collision |
775 | - - object1: lfdistal |
776 | - object2: thbase |
777 | - operation: disable # Never in collision |
778 | - - object1: lfdistal |
779 | - object2: thhub |
780 | - operation: disable # Never in collision |
781 | - - object1: lfdistal |
782 | - object2: thmiddle |
783 | - operation: disable # Never in collision |
784 | - - object1: lfdistal |
785 | - object2: thproximal |
786 | - operation: disable # Never in collision |
787 | - - object1: lfdistal |
788 | - object2: wrist |
789 | - operation: disable # Never in collision |
790 | - - object1: lfknuckle |
791 | - object2: lfmiddle |
792 | - operation: disable # Never in collision |
793 | - - object1: lfknuckle |
794 | - object2: mfdistal |
795 | - operation: disable # Never in collision |
796 | - - object1: lfknuckle |
797 | - object2: mfknuckle |
798 | - operation: disable # Never in collision |
799 | - - object1: lfknuckle |
800 | - object2: mfmiddle |
801 | - operation: disable # Never in collision |
802 | - - object1: lfknuckle |
803 | - object2: mfproximal |
804 | - operation: disable # Never in collision |
805 | - - object1: lfknuckle |
806 | - object2: palm |
807 | - operation: disable # Never in collision |
808 | - - object1: lfknuckle |
809 | - object2: rfdistal |
810 | - operation: disable # Never in collision |
811 | - - object1: lfknuckle |
812 | - object2: rfknuckle |
813 | - operation: disable # Never in collision |
814 | - - object1: lfknuckle |
815 | - object2: rfmiddle |
816 | - operation: disable # Never in collision |
817 | - - object1: lfknuckle |
818 | - object2: rfproximal |
819 | - operation: disable # Never in collision |
820 | - - object1: lfknuckle |
821 | - object2: shadowarm_base |
822 | - operation: disable # Never in collision |
823 | - - object1: lfknuckle |
824 | - object2: shadow_handsupport |
825 | - operation: disable # Never in collision |
826 | - - object1: lfknuckle |
827 | - object2: shadowarm_lowerarm |
828 | - operation: disable # Never in collision |
829 | - - object1: lfknuckle |
830 | - object2: shadowarm_trunk |
831 | - operation: disable # Never in collision |
832 | - - object1: lfknuckle |
833 | - object2: shadowarm_upperarm |
834 | - operation: disable # Never in collision |
835 | - - object1: lfknuckle |
836 | - object2: thbase |
837 | - operation: disable # Never in collision |
838 | - - object1: lfknuckle |
839 | - object2: thdistal |
840 | - operation: disable # Never in collision |
841 | - - object1: lfknuckle |
842 | - object2: thhub |
843 | - operation: disable # Never in collision |
844 | - - object1: lfknuckle |
845 | - object2: thmiddle |
846 | - operation: disable # Never in collision |
847 | - - object1: lfknuckle |
848 | - object2: thproximal |
849 | - operation: disable # Never in collision |
850 | - - object1: lfknuckle |
851 | - object2: wrist |
852 | - operation: disable # Never in collision |
853 | - - object1: lfmetacarpal |
854 | - object2: lfmiddle |
855 | - operation: disable # Never in collision |
856 | - - object1: lfmetacarpal |
857 | - object2: lfproximal |
858 | - operation: disable # Never in collision |
859 | - - object1: lfmetacarpal |
860 | - object2: mfdistal |
861 | - operation: disable # Never in collision |
862 | - - object1: lfmetacarpal |
863 | - object2: mfknuckle |
864 | - operation: disable # Never in collision |
865 | - - object1: lfmetacarpal |
866 | - object2: mfmiddle |
867 | - operation: disable # Never in collision |
868 | - - object1: lfmetacarpal |
869 | - object2: mfproximal |
870 | - operation: disable # Never in collision |
871 | - - object1: lfmetacarpal |
872 | - object2: rfdistal |
873 | - operation: disable # Never in collision |
874 | - - object1: lfmetacarpal |
875 | - object2: rfknuckle |
876 | - operation: disable # Never in collision |
877 | - - object1: lfmetacarpal |
878 | - object2: rfmiddle |
879 | - operation: disable # Never in collision |
880 | - - object1: lfmetacarpal |
881 | - object2: rfproximal |
882 | - operation: disable # Never in collision |
883 | - - object1: lfmetacarpal |
884 | - object2: shadowarm_base |
885 | - operation: disable # Never in collision |
886 | - - object1: lfmetacarpal |
887 | - object2: shadow_handsupport |
888 | - operation: disable # Never in collision |
889 | - - object1: lfmetacarpal |
890 | - object2: shadowarm_lowerarm |
891 | - operation: disable # Never in collision |
892 | - - object1: lfmetacarpal |
893 | - object2: shadowarm_trunk |
894 | - operation: disable # Never in collision |
895 | - - object1: lfmetacarpal |
896 | - object2: shadowarm_upperarm |
897 | - operation: disable # Never in collision |
898 | - - object1: lfmetacarpal |
899 | - object2: thbase |
900 | - operation: disable # Never in collision |
901 | - - object1: lfmetacarpal |
902 | - object2: thdistal |
903 | - operation: disable # Never in collision |
904 | - - object1: lfmetacarpal |
905 | - object2: thhub |
906 | - operation: disable # Never in collision |
907 | - - object1: lfmetacarpal |
908 | - object2: thmiddle |
909 | - operation: disable # Never in collision |
910 | - - object1: lfmetacarpal |
911 | - object2: thproximal |
912 | - operation: disable # Never in collision |
913 | - - object1: lfmetacarpal |
914 | - object2: wrist |
915 | - operation: disable # Never in collision |
916 | - - object1: lfmiddle |
917 | - object2: mfknuckle |
918 | - operation: disable # Never in collision |
919 | - - object1: lfmiddle |
920 | - object2: mfproximal |
921 | - operation: disable # Never in collision |
922 | - - object1: lfmiddle |
923 | - object2: palm |
924 | - operation: disable # Never in collision |
925 | - - object1: lfmiddle |
926 | - object2: rfknuckle |
927 | - operation: disable # Never in collision |
928 | - - object1: lfmiddle |
929 | - object2: shadowarm_base |
930 | - operation: disable # Never in collision |
931 | - - object1: lfmiddle |
932 | - object2: shadow_handsupport |
933 | - operation: disable # Never in collision |
934 | - - object1: lfmiddle |
935 | - object2: shadowarm_lowerarm |
936 | - operation: disable # Never in collision |
937 | - - object1: lfmiddle |
938 | - object2: shadowarm_trunk |
939 | - operation: disable # Never in collision |
940 | - - object1: lfmiddle |
941 | - object2: shadowarm_upperarm |
942 | - operation: disable # Never in collision |
943 | - - object1: lfmiddle |
944 | - object2: thbase |
945 | - operation: disable # Never in collision |
946 | - - object1: lfmiddle |
947 | - object2: thhub |
948 | - operation: disable # Never in collision |
949 | - - object1: lfmiddle |
950 | - object2: thmiddle |
951 | - operation: disable # Never in collision |
952 | - - object1: lfmiddle |
953 | - object2: thproximal |
954 | - operation: disable # Never in collision |
955 | - - object1: lfmiddle |
956 | - object2: wrist |
957 | - operation: disable # Never in collision |
958 | - - object1: lfproximal |
959 | - object2: mfknuckle |
960 | - operation: disable # Never in collision |
961 | - - object1: lfproximal |
962 | - object2: mfmiddle |
963 | - operation: disable # Never in collision |
964 | - - object1: lfproximal |
965 | - object2: mfproximal |
966 | - operation: disable # Never in collision |
967 | - - object1: lfproximal |
968 | - object2: palm |
969 | - operation: disable # Never in collision |
970 | - - object1: lfproximal |
971 | - object2: rfknuckle |
972 | - operation: disable # Never in collision |
973 | - - object1: lfproximal |
974 | - object2: shadowarm_base |
975 | - operation: disable # Never in collision |
976 | - - object1: lfproximal |
977 | - object2: shadow_handsupport |
978 | - operation: disable # Never in collision |
979 | - - object1: lfproximal |
980 | - object2: shadowarm_lowerarm |
981 | - operation: disable # Never in collision |
982 | - - object1: lfproximal |
983 | - object2: shadowarm_trunk |
984 | - operation: disable # Never in collision |
985 | - - object1: lfproximal |
986 | - object2: shadowarm_upperarm |
987 | - operation: disable # Never in collision |
988 | - - object1: lfproximal |
989 | - object2: thbase |
990 | - operation: disable # Never in collision |
991 | - - object1: lfproximal |
992 | - object2: thdistal |
993 | - operation: disable # Never in collision |
994 | - - object1: lfproximal |
995 | - object2: thhub |
996 | - operation: disable # Never in collision |
997 | - - object1: lfproximal |
998 | - object2: thmiddle |
999 | - operation: disable # Never in collision |
1000 | - - object1: lfproximal |
1001 | - object2: thproximal |
1002 | - operation: disable # Never in collision |
1003 | - - object1: lfproximal |
1004 | - object2: wrist |
1005 | - operation: disable # Never in collision |
1006 | - - object1: mfdistal |
1007 | - object2: mfknuckle |
1008 | - operation: disable # Never in collision |
1009 | - - object1: mfdistal |
1010 | - object2: mfproximal |
1011 | - operation: disable # Never in collision |
1012 | - - object1: mfdistal |
1013 | - object2: palm |
1014 | - operation: disable # Never in collision |
1015 | - - object1: mfdistal |
1016 | - object2: rfknuckle |
1017 | - operation: disable # Never in collision |
1018 | - - object1: mfdistal |
1019 | - object2: shadowarm_base |
1020 | - operation: disable # Never in collision |
1021 | - - object1: mfdistal |
1022 | - object2: shadow_handsupport |
1023 | - operation: disable # Never in collision |
1024 | - - object1: mfdistal |
1025 | - object2: shadowarm_lowerarm |
1026 | - operation: disable # Never in collision |
1027 | - - object1: mfdistal |
1028 | - object2: shadowarm_trunk |
1029 | - operation: disable # Never in collision |
1030 | - - object1: mfdistal |
1031 | - object2: shadowarm_upperarm |
1032 | - operation: disable # Never in collision |
1033 | - - object1: mfdistal |
1034 | - object2: thbase |
1035 | - operation: disable # Never in collision |
1036 | - - object1: mfdistal |
1037 | - object2: thhub |
1038 | - operation: disable # Never in collision |
1039 | - - object1: mfdistal |
1040 | - object2: thproximal |
1041 | - operation: disable # Never in collision |
1042 | - - object1: mfdistal |
1043 | - object2: wrist |
1044 | - operation: disable # Never in collision |
1045 | - - object1: mfknuckle |
1046 | - object2: mfmiddle |
1047 | - operation: disable # Never in collision |
1048 | - - object1: mfknuckle |
1049 | - object2: rfdistal |
1050 | - operation: disable # Never in collision |
1051 | - - object1: mfknuckle |
1052 | - object2: rfknuckle |
1053 | - operation: disable # Never in collision |
1054 | - - object1: mfknuckle |
1055 | - object2: rfmiddle |
1056 | - operation: disable # Never in collision |
1057 | - - object1: mfknuckle |
1058 | - object2: rfproximal |
1059 | - operation: disable # Never in collision |
1060 | - - object1: mfknuckle |
1061 | - object2: shadowarm_base |
1062 | - operation: disable # Never in collision |
1063 | - - object1: mfknuckle |
1064 | - object2: shadow_handsupport |
1065 | - operation: disable # Never in collision |
1066 | - - object1: mfknuckle |
1067 | - object2: shadowarm_lowerarm |
1068 | - operation: disable # Never in collision |
1069 | - - object1: mfknuckle |
1070 | - object2: shadowarm_trunk |
1071 | - operation: disable # Never in collision |
1072 | - - object1: mfknuckle |
1073 | - object2: shadowarm_upperarm |
1074 | - operation: disable # Never in collision |
1075 | - - object1: mfknuckle |
1076 | - object2: thbase |
1077 | - operation: disable # Never in collision |
1078 | - - object1: mfknuckle |
1079 | - object2: thhub |
1080 | - operation: disable # Never in collision |
1081 | - - object1: mfknuckle |
1082 | - object2: thmiddle |
1083 | - operation: disable # Never in collision |
1084 | - - object1: mfknuckle |
1085 | - object2: thproximal |
1086 | - operation: disable # Never in collision |
1087 | - - object1: mfknuckle |
1088 | - object2: wrist |
1089 | - operation: disable # Never in collision |
1090 | - - object1: mfmiddle |
1091 | - object2: palm |
1092 | - operation: disable # Never in collision |
1093 | - - object1: mfmiddle |
1094 | - object2: rfknuckle |
1095 | - operation: disable # Never in collision |
1096 | - - object1: mfmiddle |
1097 | - object2: rfproximal |
1098 | - operation: disable # Never in collision |
1099 | - - object1: mfmiddle |
1100 | - object2: shadowarm_base |
1101 | - operation: disable # Never in collision |
1102 | - - object1: mfmiddle |
1103 | - object2: shadow_handsupport |
1104 | - operation: disable # Never in collision |
1105 | - - object1: mfmiddle |
1106 | - object2: shadowarm_lowerarm |
1107 | - operation: disable # Never in collision |
1108 | - - object1: mfmiddle |
1109 | - object2: shadowarm_trunk |
1110 | - operation: disable # Never in collision |
1111 | - - object1: mfmiddle |
1112 | - object2: shadowarm_upperarm |
1113 | - operation: disable # Never in collision |
1114 | - - object1: mfmiddle |
1115 | - object2: thbase |
1116 | - operation: disable # Never in collision |
1117 | - - object1: mfmiddle |
1118 | - object2: thhub |
1119 | - operation: disable # Never in collision |
1120 | - - object1: mfmiddle |
1121 | - object2: thmiddle |
1122 | - operation: disable # Never in collision |
1123 | - - object1: mfmiddle |
1124 | - object2: thproximal |
1125 | - operation: disable # Never in collision |
1126 | - - object1: mfmiddle |
1127 | - object2: wrist |
1128 | - operation: disable # Never in collision |
1129 | - - object1: mfproximal |
1130 | - object2: palm |
1131 | - operation: disable # Never in collision |
1132 | - - object1: mfproximal |
1133 | - object2: rfdistal |
1134 | - operation: disable # Never in collision |
1135 | - - object1: mfproximal |
1136 | - object2: rfknuckle |
1137 | - operation: disable # Never in collision |
1138 | - - object1: mfproximal |
1139 | - object2: rfmiddle |
1140 | - operation: disable # Never in collision |
1141 | - - object1: mfproximal |
1142 | - object2: shadowarm_base |
1143 | - operation: disable # Never in collision |
1144 | - - object1: mfproximal |
1145 | - object2: shadow_handsupport |
1146 | - operation: disable # Never in collision |
1147 | - - object1: mfproximal |
1148 | - object2: shadowarm_lowerarm |
1149 | - operation: disable # Never in collision |
1150 | - - object1: mfproximal |
1151 | - object2: shadowarm_trunk |
1152 | - operation: disable # Never in collision |
1153 | - - object1: mfproximal |
1154 | - object2: shadowarm_upperarm |
1155 | - operation: disable # Never in collision |
1156 | - - object1: mfproximal |
1157 | - object2: thbase |
1158 | - operation: disable # Never in collision |
1159 | - - object1: mfproximal |
1160 | - object2: thhub |
1161 | - operation: disable # Never in collision |
1162 | - - object1: mfproximal |
1163 | - object2: thmiddle |
1164 | - operation: disable # Never in collision |
1165 | - - object1: mfproximal |
1166 | - object2: thproximal |
1167 | - operation: disable # Never in collision |
1168 | - - object1: mfproximal |
1169 | - object2: wrist |
1170 | - operation: disable # Never in collision |
1171 | - - object1: palm |
1172 | - object2: rfdistal |
1173 | - operation: disable # Never in collision |
1174 | - - object1: palm |
1175 | - object2: rfmiddle |
1176 | - operation: disable # Never in collision |
1177 | - - object1: palm |
1178 | - object2: rfproximal |
1179 | - operation: disable # Never in collision |
1180 | - - object1: palm |
1181 | - object2: shadowarm_base |
1182 | - operation: disable # Never in collision |
1183 | - - object1: palm |
1184 | - object2: shadow_handsupport |
1185 | - operation: disable # Never in collision |
1186 | - - object1: palm |
1187 | - object2: shadowarm_lowerarm |
1188 | - operation: disable # Never in collision |
1189 | - - object1: palm |
1190 | - object2: shadowarm_trunk |
1191 | - operation: disable # Never in collision |
1192 | - - object1: palm |
1193 | - object2: shadowarm_upperarm |
1194 | - operation: disable # Never in collision |
1195 | - - object1: palm |
1196 | - object2: thdistal |
1197 | - operation: disable # Never in collision |
1198 | - - object1: palm |
1199 | - object2: thhub |
1200 | - operation: disable # Never in collision |
1201 | - - object1: palm |
1202 | - object2: thmiddle |
1203 | - operation: disable # Never in collision |
1204 | - - object1: palm |
1205 | - object2: thproximal |
1206 | - operation: disable # Never in collision |
1207 | - - object1: rfdistal |
1208 | - object2: rfknuckle |
1209 | - operation: disable # Never in collision |
1210 | - - object1: rfdistal |
1211 | - object2: rfproximal |
1212 | - operation: disable # Never in collision |
1213 | - - object1: rfdistal |
1214 | - object2: shadowarm_base |
1215 | - operation: disable # Never in collision |
1216 | - - object1: rfdistal |
1217 | - object2: shadow_handsupport |
1218 | - operation: disable # Never in collision |
1219 | - - object1: rfdistal |
1220 | - object2: shadowarm_lowerarm |
1221 | - operation: disable # Never in collision |
1222 | - - object1: rfdistal |
1223 | - object2: shadowarm_trunk |
1224 | - operation: disable # Never in collision |
1225 | - - object1: rfdistal |
1226 | - object2: shadowarm_upperarm |
1227 | - operation: disable # Never in collision |
1228 | - - object1: rfdistal |
1229 | - object2: thbase |
1230 | - operation: disable # Never in collision |
1231 | - - object1: rfdistal |
1232 | - object2: thhub |
1233 | - operation: disable # Never in collision |
1234 | - - object1: rfdistal |
1235 | - object2: thmiddle |
1236 | - operation: disable # Never in collision |
1237 | - - object1: rfdistal |
1238 | - object2: thproximal |
1239 | - operation: disable # Never in collision |
1240 | - - object1: rfdistal |
1241 | - object2: wrist |
1242 | - operation: disable # Never in collision |
1243 | - - object1: rfknuckle |
1244 | - object2: rfmiddle |
1245 | - operation: disable # Never in collision |
1246 | - - object1: rfknuckle |
1247 | - object2: shadowarm_base |
1248 | - operation: disable # Never in collision |
1249 | - - object1: rfknuckle |
1250 | - object2: shadow_handsupport |
1251 | - operation: disable # Never in collision |
1252 | - - object1: rfknuckle |
1253 | - object2: shadowarm_lowerarm |
1254 | - operation: disable # Never in collision |
1255 | - - object1: rfknuckle |
1256 | - object2: shadowarm_trunk |
1257 | - operation: disable # Never in collision |
1258 | - - object1: rfknuckle |
1259 | - object2: shadowarm_upperarm |
1260 | - operation: disable # Never in collision |
1261 | - - object1: rfknuckle |
1262 | - object2: thbase |
1263 | - operation: disable # Never in collision |
1264 | - - object1: rfknuckle |
1265 | - object2: thdistal |
1266 | - operation: disable # Never in collision |
1267 | - - object1: rfknuckle |
1268 | - object2: thhub |
1269 | - operation: disable # Never in collision |
1270 | - - object1: rfknuckle |
1271 | - object2: thmiddle |
1272 | - operation: disable # Never in collision |
1273 | - - object1: rfknuckle |
1274 | - object2: thproximal |
1275 | - operation: disable # Never in collision |
1276 | - - object1: rfknuckle |
1277 | - object2: wrist |
1278 | - operation: disable # Never in collision |
1279 | - - object1: rfmiddle |
1280 | - object2: shadowarm_base |
1281 | - operation: disable # Never in collision |
1282 | - - object1: rfmiddle |
1283 | - object2: shadow_handsupport |
1284 | - operation: disable # Never in collision |
1285 | - - object1: rfmiddle |
1286 | - object2: shadowarm_lowerarm |
1287 | - operation: disable # Never in collision |
1288 | - - object1: rfmiddle |
1289 | - object2: shadowarm_trunk |
1290 | - operation: disable # Never in collision |
1291 | - - object1: rfmiddle |
1292 | - object2: shadowarm_upperarm |
1293 | - operation: disable # Never in collision |
1294 | - - object1: rfmiddle |
1295 | - object2: thbase |
1296 | - operation: disable # Never in collision |
1297 | - - object1: rfmiddle |
1298 | - object2: thdistal |
1299 | - operation: disable # Never in collision |
1300 | - - object1: rfmiddle |
1301 | - object2: thhub |
1302 | - operation: disable # Never in collision |
1303 | - - object1: rfmiddle |
1304 | - object2: thmiddle |
1305 | - operation: disable # Never in collision |
1306 | - - object1: rfmiddle |
1307 | - object2: thproximal |
1308 | - operation: disable # Never in collision |
1309 | - - object1: rfmiddle |
1310 | - object2: wrist |
1311 | - operation: disable # Never in collision |
1312 | - - object1: rfproximal |
1313 | - object2: shadowarm_base |
1314 | - operation: disable # Never in collision |
1315 | - - object1: rfproximal |
1316 | - object2: shadow_handsupport |
1317 | - operation: disable # Never in collision |
1318 | - - object1: rfproximal |
1319 | - object2: shadowarm_lowerarm |
1320 | - operation: disable # Never in collision |
1321 | - - object1: rfproximal |
1322 | - object2: shadowarm_trunk |
1323 | - operation: disable # Never in collision |
1324 | - - object1: rfproximal |
1325 | - object2: shadowarm_upperarm |
1326 | - operation: disable # Never in collision |
1327 | - - object1: rfproximal |
1328 | - object2: thbase |
1329 | - operation: disable # Never in collision |
1330 | - - object1: rfproximal |
1331 | - object2: thdistal |
1332 | - operation: disable # Never in collision |
1333 | - - object1: rfproximal |
1334 | - object2: thhub |
1335 | - operation: disable # Never in collision |
1336 | - - object1: rfproximal |
1337 | - object2: thmiddle |
1338 | - operation: disable # Never in collision |
1339 | - - object1: rfproximal |
1340 | - object2: thproximal |
1341 | - operation: disable # Never in collision |
1342 | - - object1: rfproximal |
1343 | - object2: wrist |
1344 | - operation: disable # Never in collision |
1345 | - - object1: shadowarm_base |
1346 | - object2: shadow_handsupport |
1347 | - operation: disable # Never in collision |
1348 | - - object1: shadowarm_base |
1349 | - object2: shadowarm_lowerarm |
1350 | - operation: disable # Never in collision |
1351 | - - object1: shadowarm_base |
1352 | - object2: shadowarm_upperarm |
1353 | - operation: disable # Never in collision |
1354 | - - object1: shadowarm_base |
1355 | - object2: thbase |
1356 | - operation: disable # Never in collision |
1357 | - - object1: shadowarm_base |
1358 | - object2: thdistal |
1359 | - operation: disable # Never in collision |
1360 | - - object1: shadowarm_base |
1361 | - object2: thhub |
1362 | - operation: disable # Never in collision |
1363 | - - object1: shadowarm_base |
1364 | - object2: thmiddle |
1365 | - operation: disable # Never in collision |
1366 | - - object1: shadowarm_base |
1367 | - object2: thproximal |
1368 | - operation: disable # Never in collision |
1369 | - - object1: shadowarm_base |
1370 | - object2: wrist |
1371 | - operation: disable # Never in collision |
1372 | - - object1: shadow_handsupport |
1373 | - object2: shadowarm_trunk |
1374 | - operation: disable # Never in collision |
1375 | - - object1: shadow_handsupport |
1376 | - object2: shadowarm_upperarm |
1377 | - operation: disable # Never in collision |
1378 | - - object1: shadow_handsupport |
1379 | - object2: thbase |
1380 | - operation: disable # Never in collision |
1381 | - - object1: shadow_handsupport |
1382 | - object2: thdistal |
1383 | - operation: disable # Never in collision |
1384 | - - object1: shadow_handsupport |
1385 | - object2: thhub |
1386 | - operation: disable # Never in collision |
1387 | - - object1: shadow_handsupport |
1388 | - object2: thmiddle |
1389 | - operation: disable # Never in collision |
1390 | - - object1: shadow_handsupport |
1391 | - object2: thproximal |
1392 | - operation: disable # Never in collision |
1393 | - - object1: shadow_handsupport |
1394 | + object2: ffdistal |
1395 | + operation: disable # Never in collision |
1396 | + - object1: camera_link |
1397 | + object2: ffknuckle |
1398 | + operation: disable # Never in collision |
1399 | + - object1: camera_link |
1400 | + object2: ffmiddle |
1401 | + operation: disable # Never in collision |
1402 | + - object1: camera_link |
1403 | + object2: ffproximal |
1404 | + operation: disable # Never in collision |
1405 | + - object1: camera_link |
1406 | + object2: forearm |
1407 | + operation: disable # Never in collision |
1408 | + - object1: camera_link |
1409 | + object2: lfdistal |
1410 | + operation: disable # Never in collision |
1411 | + - object1: camera_link |
1412 | + object2: lfknuckle |
1413 | + operation: disable # Never in collision |
1414 | + - object1: camera_link |
1415 | + object2: lfmetacarpal |
1416 | + operation: disable # Never in collision |
1417 | + - object1: camera_link |
1418 | + object2: lfmiddle |
1419 | + operation: disable # Never in collision |
1420 | + - object1: camera_link |
1421 | + object2: lfproximal |
1422 | + operation: disable # Never in collision |
1423 | + - object1: camera_link |
1424 | + object2: mfdistal |
1425 | + operation: disable # Never in collision |
1426 | + - object1: camera_link |
1427 | + object2: mfknuckle |
1428 | + operation: disable # Never in collision |
1429 | + - object1: camera_link |
1430 | + object2: mfmiddle |
1431 | + operation: disable # Never in collision |
1432 | + - object1: camera_link |
1433 | + object2: mfproximal |
1434 | + operation: disable # Never in collision |
1435 | + - object1: camera_link |
1436 | + object2: palm |
1437 | + operation: disable # Never in collision |
1438 | + - object1: camera_link |
1439 | + object2: rfdistal |
1440 | + operation: disable # Never in collision |
1441 | + - object1: camera_link |
1442 | + object2: rfknuckle |
1443 | + operation: disable # Never in collision |
1444 | + - object1: camera_link |
1445 | + object2: rfmiddle |
1446 | + operation: disable # Never in collision |
1447 | + - object1: camera_link |
1448 | + object2: rfproximal |
1449 | + operation: disable # Never in collision |
1450 | + - object1: camera_link |
1451 | + object2: shadowarm_base |
1452 | + operation: disable # Never in collision |
1453 | + - object1: camera_link |
1454 | + object2: shadowarm_handsupport |
1455 | + operation: disable # Never in collision |
1456 | + - object1: camera_link |
1457 | + object2: shadowarm_lowerarm |
1458 | + operation: disable # Never in collision |
1459 | + - object1: camera_link |
1460 | + object2: shadowarm_upperarm |
1461 | + operation: disable # Never in collision |
1462 | + - object1: camera_link |
1463 | + object2: thbase |
1464 | + operation: disable # Never in collision |
1465 | + - object1: camera_link |
1466 | + object2: thdistal |
1467 | + operation: disable # Never in collision |
1468 | + - object1: camera_link |
1469 | + object2: thhub |
1470 | + operation: disable # Never in collision |
1471 | + - object1: camera_link |
1472 | + object2: thmiddle |
1473 | + operation: disable # Never in collision |
1474 | + - object1: camera_link |
1475 | + object2: thproximal |
1476 | + operation: disable # Never in collision |
1477 | + - object1: camera_link |
1478 | + object2: wrist |
1479 | + operation: disable # Never in collision |
1480 | + object2: shadowarm_trunk |
1481 | + - object1: ffdistal |
1482 | + object2: ffknuckle |
1483 | + operation: disable # Never in collision |
1484 | + - object1: ffdistal |
1485 | + object2: ffproximal |
1486 | + operation: disable # Never in collision |
1487 | + - object1: ffdistal |
1488 | + object2: forearm |
1489 | + operation: disable # Never in collision |
1490 | + - object1: ffdistal |
1491 | + object2: lfknuckle |
1492 | + operation: disable # Never in collision |
1493 | + - object1: ffdistal |
1494 | + object2: lfmetacarpal |
1495 | + operation: disable # Never in collision |
1496 | + - object1: ffdistal |
1497 | + object2: lfmiddle |
1498 | + operation: disable # Never in collision |
1499 | + - object1: ffdistal |
1500 | + object2: lfproximal |
1501 | + operation: disable # Never in collision |
1502 | + - object1: ffdistal |
1503 | + object2: mfknuckle |
1504 | + operation: disable # Never in collision |
1505 | + - object1: ffdistal |
1506 | + object2: mfproximal |
1507 | + operation: disable # Never in collision |
1508 | + - object1: ffdistal |
1509 | + object2: palm |
1510 | + operation: disable # Never in collision |
1511 | + - object1: ffdistal |
1512 | + object2: rfknuckle |
1513 | + operation: disable # Never in collision |
1514 | + - object1: ffdistal |
1515 | + object2: rfmiddle |
1516 | + operation: disable # Never in collision |
1517 | + - object1: ffdistal |
1518 | + object2: rfproximal |
1519 | + operation: disable # Never in collision |
1520 | + - object1: ffdistal |
1521 | + object2: shadowarm_base |
1522 | + operation: disable # Never in collision |
1523 | + - object1: ffdistal |
1524 | + object2: shadowarm_handsupport |
1525 | + operation: disable # Never in collision |
1526 | + - object1: ffdistal |
1527 | + object2: shadowarm_lowerarm |
1528 | + operation: disable # Never in collision |
1529 | + - object1: ffdistal |
1530 | + object2: shadowarm_trunk |
1531 | + operation: disable # Never in collision |
1532 | + - object1: ffdistal |
1533 | + object2: shadowarm_upperarm |
1534 | + operation: disable # Never in collision |
1535 | + - object1: ffdistal |
1536 | + object2: thbase |
1537 | + operation: disable # Never in collision |
1538 | + - object1: ffdistal |
1539 | + object2: thhub |
1540 | + operation: disable # Never in collision |
1541 | + - object1: ffdistal |
1542 | + object2: thproximal |
1543 | + operation: disable # Never in collision |
1544 | + - object1: ffdistal |
1545 | + object2: wrist |
1546 | + operation: disable # Never in collision |
1547 | + - object1: ffknuckle |
1548 | + object2: ffmiddle |
1549 | + operation: disable # Never in collision |
1550 | + - object1: ffknuckle |
1551 | + object2: forearm |
1552 | + operation: disable # Never in collision |
1553 | + - object1: ffknuckle |
1554 | + object2: lfdistal |
1555 | + operation: disable # Never in collision |
1556 | + - object1: ffknuckle |
1557 | + object2: lfknuckle |
1558 | + operation: disable # Never in collision |
1559 | + - object1: ffknuckle |
1560 | + object2: lfmetacarpal |
1561 | + operation: disable # Never in collision |
1562 | + - object1: ffknuckle |
1563 | + object2: lfmiddle |
1564 | + operation: disable # Never in collision |
1565 | + - object1: ffknuckle |
1566 | + object2: lfproximal |
1567 | + operation: disable # Never in collision |
1568 | + - object1: ffknuckle |
1569 | + object2: mfdistal |
1570 | + operation: disable # Never in collision |
1571 | + - object1: ffknuckle |
1572 | + object2: mfknuckle |
1573 | + operation: disable # Never in collision |
1574 | + - object1: ffknuckle |
1575 | + object2: mfmiddle |
1576 | + operation: disable # Never in collision |
1577 | + - object1: ffknuckle |
1578 | + object2: mfproximal |
1579 | + operation: disable # Never in collision |
1580 | + - object1: ffknuckle |
1581 | + object2: rfdistal |
1582 | + operation: disable # Never in collision |
1583 | + - object1: ffknuckle |
1584 | + object2: rfknuckle |
1585 | + operation: disable # Never in collision |
1586 | + - object1: ffknuckle |
1587 | + object2: rfmiddle |
1588 | + operation: disable # Never in collision |
1589 | + - object1: ffknuckle |
1590 | + object2: rfproximal |
1591 | + operation: disable # Never in collision |
1592 | + - object1: ffknuckle |
1593 | + object2: shadowarm_base |
1594 | + operation: disable # Never in collision |
1595 | + - object1: ffknuckle |
1596 | + object2: shadowarm_handsupport |
1597 | + operation: disable # Never in collision |
1598 | + - object1: ffknuckle |
1599 | + object2: shadowarm_lowerarm |
1600 | + operation: disable # Never in collision |
1601 | + - object1: ffknuckle |
1602 | + object2: shadowarm_trunk |
1603 | + operation: disable # Never in collision |
1604 | + - object1: ffknuckle |
1605 | + object2: shadowarm_upperarm |
1606 | + operation: disable # Never in collision |
1607 | + - object1: ffknuckle |
1608 | + object2: thbase |
1609 | + operation: disable # Never in collision |
1610 | + - object1: ffknuckle |
1611 | + object2: thhub |
1612 | + operation: disable # Never in collision |
1613 | + - object1: ffknuckle |
1614 | + object2: thmiddle |
1615 | + operation: disable # Never in collision |
1616 | + - object1: ffknuckle |
1617 | + object2: thproximal |
1618 | + operation: disable # Never in collision |
1619 | + - object1: ffknuckle |
1620 | + object2: wrist |
1621 | + operation: disable # Never in collision |
1622 | + - object1: ffmiddle |
1623 | + object2: forearm |
1624 | + operation: disable # Never in collision |
1625 | + - object1: ffmiddle |
1626 | + object2: lfdistal |
1627 | + operation: disable # Never in collision |
1628 | + - object1: ffmiddle |
1629 | + object2: lfknuckle |
1630 | + operation: disable # Never in collision |
1631 | + - object1: ffmiddle |
1632 | + object2: lfmetacarpal |
1633 | + operation: disable # Never in collision |
1634 | + - object1: ffmiddle |
1635 | + object2: lfmiddle |
1636 | + operation: disable # Never in collision |
1637 | + - object1: ffmiddle |
1638 | + object2: lfproximal |
1639 | + operation: disable # Never in collision |
1640 | + - object1: ffmiddle |
1641 | + object2: mfknuckle |
1642 | + operation: disable # Never in collision |
1643 | + - object1: ffmiddle |
1644 | + object2: palm |
1645 | + operation: disable # Never in collision |
1646 | + - object1: ffmiddle |
1647 | + object2: rfdistal |
1648 | + operation: disable # Never in collision |
1649 | + - object1: ffmiddle |
1650 | + object2: rfknuckle |
1651 | + operation: disable # Never in collision |
1652 | + - object1: ffmiddle |
1653 | + object2: rfmiddle |
1654 | + operation: disable # Never in collision |
1655 | + - object1: ffmiddle |
1656 | + object2: rfproximal |
1657 | + operation: disable # Never in collision |
1658 | + - object1: ffmiddle |
1659 | + object2: shadowarm_base |
1660 | + operation: disable # Never in collision |
1661 | + - object1: ffmiddle |
1662 | + object2: shadowarm_handsupport |
1663 | + operation: disable # Never in collision |
1664 | + - object1: ffmiddle |
1665 | + object2: shadowarm_lowerarm |
1666 | + operation: disable # Never in collision |
1667 | + - object1: ffmiddle |
1668 | + object2: shadowarm_trunk |
1669 | + operation: disable # Never in collision |
1670 | + - object1: ffmiddle |
1671 | + object2: shadowarm_upperarm |
1672 | + operation: disable # Never in collision |
1673 | + - object1: ffmiddle |
1674 | + object2: thbase |
1675 | + operation: disable # Never in collision |
1676 | + - object1: ffmiddle |
1677 | + object2: thhub |
1678 | + operation: disable # Never in collision |
1679 | + - object1: ffmiddle |
1680 | + object2: thmiddle |
1681 | + operation: disable # Never in collision |
1682 | + - object1: ffmiddle |
1683 | + object2: thproximal |
1684 | + operation: disable # Never in collision |
1685 | + - object1: ffmiddle |
1686 | + object2: wrist |
1687 | + operation: disable # Never in collision |
1688 | + - object1: ffproximal |
1689 | + object2: forearm |
1690 | + operation: disable # Never in collision |
1691 | + - object1: ffproximal |
1692 | + object2: lfdistal |
1693 | + operation: disable # Never in collision |
1694 | + - object1: ffproximal |
1695 | + object2: lfknuckle |
1696 | + operation: disable # Never in collision |
1697 | + - object1: ffproximal |
1698 | + object2: lfmetacarpal |
1699 | + operation: disable # Never in collision |
1700 | + - object1: ffproximal |
1701 | + object2: lfmiddle |
1702 | + operation: disable # Never in collision |
1703 | + - object1: ffproximal |
1704 | + object2: lfproximal |
1705 | + operation: disable # Never in collision |
1706 | + - object1: ffproximal |
1707 | + object2: mfdistal |
1708 | + operation: disable # Never in collision |
1709 | + - object1: ffproximal |
1710 | + object2: mfknuckle |
1711 | + operation: disable # Never in collision |
1712 | + - object1: ffproximal |
1713 | + object2: mfmiddle |
1714 | + operation: disable # Never in collision |
1715 | + - object1: ffproximal |
1716 | + object2: palm |
1717 | + operation: disable # Never in collision |
1718 | + - object1: ffproximal |
1719 | + object2: rfdistal |
1720 | + operation: disable # Never in collision |
1721 | + - object1: ffproximal |
1722 | + object2: rfknuckle |
1723 | + operation: disable # Never in collision |
1724 | + - object1: ffproximal |
1725 | + object2: rfmiddle |
1726 | + operation: disable # Never in collision |
1727 | + - object1: ffproximal |
1728 | + object2: rfproximal |
1729 | + operation: disable # Never in collision |
1730 | + - object1: ffproximal |
1731 | + object2: shadowarm_base |
1732 | + operation: disable # Never in collision |
1733 | + - object1: ffproximal |
1734 | + object2: shadowarm_handsupport |
1735 | + operation: disable # Never in collision |
1736 | + - object1: ffproximal |
1737 | + object2: shadowarm_lowerarm |
1738 | + operation: disable # Never in collision |
1739 | + - object1: ffproximal |
1740 | + object2: shadowarm_trunk |
1741 | + operation: disable # Never in collision |
1742 | + - object1: ffproximal |
1743 | + object2: shadowarm_upperarm |
1744 | + operation: disable # Never in collision |
1745 | + - object1: ffproximal |
1746 | + object2: thbase |
1747 | + operation: disable # Never in collision |
1748 | + - object1: ffproximal |
1749 | + object2: thhub |
1750 | + operation: disable # Never in collision |
1751 | + - object1: ffproximal |
1752 | + object2: thmiddle |
1753 | + operation: disable # Never in collision |
1754 | + - object1: ffproximal |
1755 | + object2: thproximal |
1756 | + operation: disable # Never in collision |
1757 | + - object1: ffproximal |
1758 | + object2: wrist |
1759 | + operation: disable # Never in collision |
1760 | + - object1: forearm |
1761 | + object2: lfdistal |
1762 | + operation: disable # Never in collision |
1763 | + - object1: forearm |
1764 | + object2: lfknuckle |
1765 | + operation: disable # Never in collision |
1766 | + - object1: forearm |
1767 | + object2: lfmetacarpal |
1768 | + operation: disable # Never in collision |
1769 | + - object1: forearm |
1770 | + object2: lfmiddle |
1771 | + operation: disable # Never in collision |
1772 | + - object1: forearm |
1773 | + object2: lfproximal |
1774 | + operation: disable # Never in collision |
1775 | + - object1: forearm |
1776 | + object2: mfdistal |
1777 | + operation: disable # Never in collision |
1778 | + - object1: forearm |
1779 | + object2: mfknuckle |
1780 | + operation: disable # Never in collision |
1781 | + - object1: forearm |
1782 | + object2: mfmiddle |
1783 | + operation: disable # Never in collision |
1784 | + - object1: forearm |
1785 | + object2: mfproximal |
1786 | + operation: disable # Never in collision |
1787 | + - object1: forearm |
1788 | + object2: palm |
1789 | + operation: disable # Never in collision |
1790 | + - object1: forearm |
1791 | + object2: rfdistal |
1792 | + operation: disable # Never in collision |
1793 | + - object1: forearm |
1794 | + object2: rfknuckle |
1795 | + operation: disable # Never in collision |
1796 | + - object1: forearm |
1797 | + object2: rfmiddle |
1798 | + operation: disable # Never in collision |
1799 | + - object1: forearm |
1800 | + object2: rfproximal |
1801 | + operation: disable # Never in collision |
1802 | + - object1: forearm |
1803 | + object2: shadowarm_base |
1804 | + operation: disable # Never in collision |
1805 | + - object1: forearm |
1806 | + object2: shadowarm_trunk |
1807 | + operation: disable # Never in collision |
1808 | + - object1: forearm |
1809 | + object2: shadowarm_upperarm |
1810 | + operation: disable # Never in collision |
1811 | + - object1: forearm |
1812 | + object2: thbase |
1813 | + operation: disable # Never in collision |
1814 | + - object1: forearm |
1815 | + object2: thdistal |
1816 | + operation: disable # Never in collision |
1817 | + - object1: forearm |
1818 | + object2: thhub |
1819 | + operation: disable # Never in collision |
1820 | + - object1: forearm |
1821 | + object2: thmiddle |
1822 | + operation: disable # Never in collision |
1823 | + - object1: forearm |
1824 | + object2: thproximal |
1825 | + operation: disable # Never in collision |
1826 | + - object1: lfdistal |
1827 | + object2: lfknuckle |
1828 | + operation: disable # Never in collision |
1829 | + - object1: lfdistal |
1830 | + object2: lfmetacarpal |
1831 | + operation: disable # Never in collision |
1832 | + - object1: lfdistal |
1833 | + object2: lfproximal |
1834 | + operation: disable # Never in collision |
1835 | + - object1: lfdistal |
1836 | + object2: mfknuckle |
1837 | + operation: disable # Never in collision |
1838 | + - object1: lfdistal |
1839 | + object2: mfproximal |
1840 | + operation: disable # Never in collision |
1841 | + - object1: lfdistal |
1842 | + object2: palm |
1843 | + operation: disable # Never in collision |
1844 | + - object1: lfdistal |
1845 | + object2: rfknuckle |
1846 | + operation: disable # Never in collision |
1847 | + - object1: lfdistal |
1848 | + object2: shadowarm_base |
1849 | + operation: disable # Never in collision |
1850 | + - object1: lfdistal |
1851 | + object2: shadowarm_handsupport |
1852 | + operation: disable # Never in collision |
1853 | + - object1: lfdistal |
1854 | + object2: shadowarm_lowerarm |
1855 | + operation: disable # Never in collision |
1856 | + - object1: lfdistal |
1857 | + object2: shadowarm_trunk |
1858 | + operation: disable # Never in collision |
1859 | + - object1: lfdistal |
1860 | + object2: shadowarm_upperarm |
1861 | + operation: disable # Never in collision |
1862 | + - object1: lfdistal |
1863 | + object2: thbase |
1864 | + operation: disable # Never in collision |
1865 | + - object1: lfdistal |
1866 | + object2: thhub |
1867 | + operation: disable # Never in collision |
1868 | + - object1: lfdistal |
1869 | + object2: thmiddle |
1870 | + operation: disable # Never in collision |
1871 | + - object1: lfdistal |
1872 | + object2: thproximal |
1873 | + operation: disable # Never in collision |
1874 | + - object1: lfdistal |
1875 | + object2: wrist |
1876 | + operation: disable # Never in collision |
1877 | + - object1: lfknuckle |
1878 | + object2: lfmiddle |
1879 | + operation: disable # Never in collision |
1880 | + - object1: lfknuckle |
1881 | + object2: mfdistal |
1882 | + operation: disable # Never in collision |
1883 | + - object1: lfknuckle |
1884 | + object2: mfknuckle |
1885 | + operation: disable # Never in collision |
1886 | + - object1: lfknuckle |
1887 | + object2: mfmiddle |
1888 | + operation: disable # Never in collision |
1889 | + - object1: lfknuckle |
1890 | + object2: mfproximal |
1891 | + operation: disable # Never in collision |
1892 | + - object1: lfknuckle |
1893 | + object2: palm |
1894 | + operation: disable # Never in collision |
1895 | + - object1: lfknuckle |
1896 | + object2: rfdistal |
1897 | + operation: disable # Never in collision |
1898 | + - object1: lfknuckle |
1899 | + object2: rfknuckle |
1900 | + operation: disable # Never in collision |
1901 | + - object1: lfknuckle |
1902 | + object2: rfmiddle |
1903 | + operation: disable # Never in collision |
1904 | + - object1: lfknuckle |
1905 | + object2: rfproximal |
1906 | + operation: disable # Never in collision |
1907 | + - object1: lfknuckle |
1908 | + object2: shadowarm_base |
1909 | + operation: disable # Never in collision |
1910 | + - object1: lfknuckle |
1911 | + object2: shadowarm_handsupport |
1912 | + operation: disable # Never in collision |
1913 | + - object1: lfknuckle |
1914 | + object2: shadowarm_lowerarm |
1915 | + operation: disable # Never in collision |
1916 | + - object1: lfknuckle |
1917 | + object2: shadowarm_trunk |
1918 | + operation: disable # Never in collision |
1919 | + - object1: lfknuckle |
1920 | + object2: shadowarm_upperarm |
1921 | + operation: disable # Never in collision |
1922 | + - object1: lfknuckle |
1923 | + object2: thbase |
1924 | + operation: disable # Never in collision |
1925 | + - object1: lfknuckle |
1926 | + object2: thdistal |
1927 | + operation: disable # Never in collision |
1928 | + - object1: lfknuckle |
1929 | + object2: thhub |
1930 | + operation: disable # Never in collision |
1931 | + - object1: lfknuckle |
1932 | + object2: thmiddle |
1933 | + operation: disable # Never in collision |
1934 | + - object1: lfknuckle |
1935 | + object2: thproximal |
1936 | + operation: disable # Never in collision |
1937 | + - object1: lfknuckle |
1938 | + object2: wrist |
1939 | + operation: disable # Never in collision |
1940 | + - object1: lfmetacarpal |
1941 | + object2: lfmiddle |
1942 | + operation: disable # Never in collision |
1943 | + - object1: lfmetacarpal |
1944 | + object2: lfproximal |
1945 | + operation: disable # Never in collision |
1946 | + - object1: lfmetacarpal |
1947 | + object2: mfdistal |
1948 | + operation: disable # Never in collision |
1949 | + - object1: lfmetacarpal |
1950 | + object2: mfknuckle |
1951 | + operation: disable # Never in collision |
1952 | + - object1: lfmetacarpal |
1953 | + object2: mfmiddle |
1954 | + operation: disable # Never in collision |
1955 | + - object1: lfmetacarpal |
1956 | + object2: mfproximal |
1957 | + operation: disable # Never in collision |
1958 | + - object1: lfmetacarpal |
1959 | + object2: rfdistal |
1960 | + operation: disable # Never in collision |
1961 | + - object1: lfmetacarpal |
1962 | + object2: rfknuckle |
1963 | + operation: disable # Never in collision |
1964 | + - object1: lfmetacarpal |
1965 | + object2: rfmiddle |
1966 | + operation: disable # Never in collision |
1967 | + - object1: lfmetacarpal |
1968 | + object2: rfproximal |
1969 | + operation: disable # Never in collision |
1970 | + - object1: lfmetacarpal |
1971 | + object2: shadowarm_base |
1972 | + operation: disable # Never in collision |
1973 | + - object1: lfmetacarpal |
1974 | + object2: shadowarm_handsupport |
1975 | + operation: disable # Never in collision |
1976 | + - object1: lfmetacarpal |
1977 | + object2: shadowarm_lowerarm |
1978 | + operation: disable # Never in collision |
1979 | + - object1: lfmetacarpal |
1980 | + object2: shadowarm_trunk |
1981 | + operation: disable # Never in collision |
1982 | + - object1: lfmetacarpal |
1983 | + object2: shadowarm_upperarm |
1984 | + operation: disable # Never in collision |
1985 | + - object1: lfmetacarpal |
1986 | + object2: thbase |
1987 | + operation: disable # Never in collision |
1988 | + - object1: lfmetacarpal |
1989 | + object2: thdistal |
1990 | + operation: disable # Never in collision |
1991 | + - object1: lfmetacarpal |
1992 | + object2: thhub |
1993 | + operation: disable # Never in collision |
1994 | + - object1: lfmetacarpal |
1995 | + object2: thmiddle |
1996 | + operation: disable # Never in collision |
1997 | + - object1: lfmetacarpal |
1998 | + object2: thproximal |
1999 | + operation: disable # Never in collision |
2000 | + - object1: lfmetacarpal |
2001 | + object2: wrist |
2002 | + operation: disable # Never in collision |
2003 | + - object1: lfmiddle |
2004 | + object2: mfknuckle |
2005 | + operation: disable # Never in collision |
2006 | + - object1: lfmiddle |
2007 | + object2: mfproximal |
2008 | + operation: disable # Never in collision |
2009 | + - object1: lfmiddle |
2010 | + object2: palm |
2011 | + operation: disable # Never in collision |
2012 | + - object1: lfmiddle |
2013 | + object2: rfknuckle |
2014 | + operation: disable # Never in collision |
2015 | + - object1: lfmiddle |
2016 | + object2: shadowarm_base |
2017 | + operation: disable # Never in collision |
2018 | + - object1: lfmiddle |
2019 | + object2: shadowarm_handsupport |
2020 | + operation: disable # Never in collision |
2021 | + - object1: lfmiddle |
2022 | + object2: shadowarm_lowerarm |
2023 | + operation: disable # Never in collision |
2024 | + - object1: lfmiddle |
2025 | + object2: shadowarm_trunk |
2026 | + operation: disable # Never in collision |
2027 | + - object1: lfmiddle |
2028 | + object2: shadowarm_upperarm |
2029 | + operation: disable # Never in collision |
2030 | + - object1: lfmiddle |
2031 | + object2: thbase |
2032 | + operation: disable # Never in collision |
2033 | + - object1: lfmiddle |
2034 | + object2: thhub |
2035 | + operation: disable # Never in collision |
2036 | + - object1: lfmiddle |
2037 | + object2: thmiddle |
2038 | + operation: disable # Never in collision |
2039 | + - object1: lfmiddle |
2040 | + object2: thproximal |
2041 | + operation: disable # Never in collision |
2042 | + - object1: lfmiddle |
2043 | + object2: wrist |
2044 | + operation: disable # Never in collision |
2045 | + - object1: lfproximal |
2046 | + object2: mfknuckle |
2047 | + operation: disable # Never in collision |
2048 | + - object1: lfproximal |
2049 | + object2: mfmiddle |
2050 | + operation: disable # Never in collision |
2051 | + - object1: lfproximal |
2052 | + object2: mfproximal |
2053 | + operation: disable # Never in collision |
2054 | + - object1: lfproximal |
2055 | + object2: palm |
2056 | + operation: disable # Never in collision |
2057 | + - object1: lfproximal |
2058 | + object2: rfknuckle |
2059 | + operation: disable # Never in collision |
2060 | + - object1: lfproximal |
2061 | + object2: shadowarm_base |
2062 | + operation: disable # Never in collision |
2063 | + - object1: lfproximal |
2064 | + object2: shadowarm_handsupport |
2065 | + operation: disable # Never in collision |
2066 | + - object1: lfproximal |
2067 | + object2: shadowarm_lowerarm |
2068 | + operation: disable # Never in collision |
2069 | + - object1: lfproximal |
2070 | + object2: shadowarm_trunk |
2071 | + operation: disable # Never in collision |
2072 | + - object1: lfproximal |
2073 | + object2: shadowarm_upperarm |
2074 | + operation: disable # Never in collision |
2075 | + - object1: lfproximal |
2076 | + object2: thbase |
2077 | + operation: disable # Never in collision |
2078 | + - object1: lfproximal |
2079 | + object2: thdistal |
2080 | + operation: disable # Never in collision |
2081 | + - object1: lfproximal |
2082 | + object2: thhub |
2083 | + operation: disable # Never in collision |
2084 | + - object1: lfproximal |
2085 | + object2: thmiddle |
2086 | + operation: disable # Never in collision |
2087 | + - object1: lfproximal |
2088 | + object2: thproximal |
2089 | + operation: disable # Never in collision |
2090 | + - object1: lfproximal |
2091 | + object2: wrist |
2092 | + operation: disable # Never in collision |
2093 | + - object1: mfdistal |
2094 | + object2: mfknuckle |
2095 | + operation: disable # Never in collision |
2096 | + - object1: mfdistal |
2097 | + object2: mfproximal |
2098 | + operation: disable # Never in collision |
2099 | + - object1: mfdistal |
2100 | + object2: palm |
2101 | + operation: disable # Never in collision |
2102 | + - object1: mfdistal |
2103 | + object2: rfknuckle |
2104 | + operation: disable # Never in collision |
2105 | + - object1: mfdistal |
2106 | + object2: shadowarm_base |
2107 | + operation: disable # Never in collision |
2108 | + - object1: mfdistal |
2109 | + object2: shadowarm_handsupport |
2110 | + operation: disable # Never in collision |
2111 | + - object1: mfdistal |
2112 | + object2: shadowarm_lowerarm |
2113 | + operation: disable # Never in collision |
2114 | + - object1: mfdistal |
2115 | + object2: shadowarm_trunk |
2116 | + operation: disable # Never in collision |
2117 | + - object1: mfdistal |
2118 | + object2: shadowarm_upperarm |
2119 | + operation: disable # Never in collision |
2120 | + - object1: mfdistal |
2121 | + object2: thbase |
2122 | + operation: disable # Never in collision |
2123 | + - object1: mfdistal |
2124 | + object2: thhub |
2125 | + operation: disable # Never in collision |
2126 | + - object1: mfdistal |
2127 | + object2: thproximal |
2128 | + operation: disable # Never in collision |
2129 | + - object1: mfdistal |
2130 | + object2: wrist |
2131 | + operation: disable # Never in collision |
2132 | + - object1: mfknuckle |
2133 | + object2: mfmiddle |
2134 | + operation: disable # Never in collision |
2135 | + - object1: mfknuckle |
2136 | + object2: rfdistal |
2137 | + operation: disable # Never in collision |
2138 | + - object1: mfknuckle |
2139 | + object2: rfknuckle |
2140 | + operation: disable # Never in collision |
2141 | + - object1: mfknuckle |
2142 | + object2: rfmiddle |
2143 | + operation: disable # Never in collision |
2144 | + - object1: mfknuckle |
2145 | + object2: rfproximal |
2146 | + operation: disable # Never in collision |
2147 | + - object1: mfknuckle |
2148 | + object2: shadowarm_base |
2149 | + operation: disable # Never in collision |
2150 | + - object1: mfknuckle |
2151 | + object2: shadowarm_handsupport |
2152 | + operation: disable # Never in collision |
2153 | + - object1: mfknuckle |
2154 | + object2: shadowarm_lowerarm |
2155 | + operation: disable # Never in collision |
2156 | + - object1: mfknuckle |
2157 | + object2: shadowarm_trunk |
2158 | + operation: disable # Never in collision |
2159 | + - object1: mfknuckle |
2160 | + object2: shadowarm_upperarm |
2161 | + operation: disable # Never in collision |
2162 | + - object1: mfknuckle |
2163 | + object2: thbase |
2164 | + operation: disable # Never in collision |
2165 | + - object1: mfknuckle |
2166 | + object2: thhub |
2167 | + operation: disable # Never in collision |
2168 | + - object1: mfknuckle |
2169 | + object2: thmiddle |
2170 | + operation: disable # Never in collision |
2171 | + - object1: mfknuckle |
2172 | + object2: thproximal |
2173 | + operation: disable # Never in collision |
2174 | + - object1: mfknuckle |
2175 | + object2: wrist |
2176 | + operation: disable # Never in collision |
2177 | + - object1: mfmiddle |
2178 | + object2: palm |
2179 | + operation: disable # Never in collision |
2180 | + - object1: mfmiddle |
2181 | + object2: rfknuckle |
2182 | + operation: disable # Never in collision |
2183 | + - object1: mfmiddle |
2184 | + object2: rfproximal |
2185 | + operation: disable # Never in collision |
2186 | + - object1: mfmiddle |
2187 | + object2: shadowarm_base |
2188 | + operation: disable # Never in collision |
2189 | + - object1: mfmiddle |
2190 | + object2: shadowarm_handsupport |
2191 | + operation: disable # Never in collision |
2192 | + - object1: mfmiddle |
2193 | + object2: shadowarm_lowerarm |
2194 | + operation: disable # Never in collision |
2195 | + - object1: mfmiddle |
2196 | + object2: shadowarm_trunk |
2197 | + operation: disable # Never in collision |
2198 | + - object1: mfmiddle |
2199 | + object2: shadowarm_upperarm |
2200 | + operation: disable # Never in collision |
2201 | + - object1: mfmiddle |
2202 | + object2: thbase |
2203 | + operation: disable # Never in collision |
2204 | + - object1: mfmiddle |
2205 | + object2: thhub |
2206 | + operation: disable # Never in collision |
2207 | + - object1: mfmiddle |
2208 | + object2: thmiddle |
2209 | + operation: disable # Never in collision |
2210 | + - object1: mfmiddle |
2211 | + object2: thproximal |
2212 | + operation: disable # Never in collision |
2213 | + - object1: mfmiddle |
2214 | + object2: wrist |
2215 | + operation: disable # Never in collision |
2216 | + - object1: mfproximal |
2217 | + object2: palm |
2218 | + operation: disable # Never in collision |
2219 | + - object1: mfproximal |
2220 | + object2: rfdistal |
2221 | + operation: disable # Never in collision |
2222 | + - object1: mfproximal |
2223 | + object2: rfknuckle |
2224 | + operation: disable # Never in collision |
2225 | + - object1: mfproximal |
2226 | + object2: rfmiddle |
2227 | + operation: disable # Never in collision |
2228 | + - object1: mfproximal |
2229 | + object2: shadowarm_base |
2230 | + operation: disable # Never in collision |
2231 | + - object1: mfproximal |
2232 | + object2: shadowarm_handsupport |
2233 | + operation: disable # Never in collision |
2234 | + - object1: mfproximal |
2235 | + object2: shadowarm_lowerarm |
2236 | + operation: disable # Never in collision |
2237 | + - object1: mfproximal |
2238 | + object2: shadowarm_trunk |
2239 | + operation: disable # Never in collision |
2240 | + - object1: mfproximal |
2241 | + object2: shadowarm_upperarm |
2242 | + operation: disable # Never in collision |
2243 | + - object1: mfproximal |
2244 | + object2: thbase |
2245 | + operation: disable # Never in collision |
2246 | + - object1: mfproximal |
2247 | + object2: thhub |
2248 | + operation: disable # Never in collision |
2249 | + - object1: mfproximal |
2250 | + object2: thmiddle |
2251 | + operation: disable # Never in collision |
2252 | + - object1: mfproximal |
2253 | + object2: thproximal |
2254 | + operation: disable # Never in collision |
2255 | + - object1: mfproximal |
2256 | + object2: wrist |
2257 | + operation: disable # Never in collision |
2258 | + - object1: palm |
2259 | + object2: rfdistal |
2260 | + operation: disable # Never in collision |
2261 | + - object1: palm |
2262 | + object2: rfmiddle |
2263 | + operation: disable # Never in collision |
2264 | + - object1: palm |
2265 | + object2: rfproximal |
2266 | + operation: disable # Never in collision |
2267 | + - object1: palm |
2268 | + object2: shadowarm_base |
2269 | + operation: disable # Never in collision |
2270 | + - object1: palm |
2271 | + object2: shadowarm_handsupport |
2272 | + operation: disable # Never in collision |
2273 | + - object1: palm |
2274 | + object2: shadowarm_lowerarm |
2275 | + operation: disable # Never in collision |
2276 | + - object1: palm |
2277 | + object2: shadowarm_trunk |
2278 | + operation: disable # Never in collision |
2279 | + - object1: palm |
2280 | + object2: shadowarm_upperarm |
2281 | + operation: disable # Never in collision |
2282 | + - object1: palm |
2283 | + object2: thdistal |
2284 | + operation: disable # Never in collision |
2285 | + - object1: palm |
2286 | + object2: thhub |
2287 | + operation: disable # Never in collision |
2288 | + - object1: palm |
2289 | + object2: thmiddle |
2290 | + operation: disable # Never in collision |
2291 | + - object1: palm |
2292 | + object2: thproximal |
2293 | + operation: disable # Never in collision |
2294 | + - object1: rfdistal |
2295 | + object2: rfknuckle |
2296 | + operation: disable # Never in collision |
2297 | + - object1: rfdistal |
2298 | + object2: rfproximal |
2299 | + operation: disable # Never in collision |
2300 | + - object1: rfdistal |
2301 | + object2: shadowarm_base |
2302 | + operation: disable # Never in collision |
2303 | + - object1: rfdistal |
2304 | + object2: shadowarm_handsupport |
2305 | + operation: disable # Never in collision |
2306 | + - object1: rfdistal |
2307 | + object2: shadowarm_lowerarm |
2308 | + operation: disable # Never in collision |
2309 | + - object1: rfdistal |
2310 | + object2: shadowarm_trunk |
2311 | + operation: disable # Never in collision |
2312 | + - object1: rfdistal |
2313 | + object2: shadowarm_upperarm |
2314 | + operation: disable # Never in collision |
2315 | + - object1: rfdistal |
2316 | + object2: thbase |
2317 | + operation: disable # Never in collision |
2318 | + - object1: rfdistal |
2319 | + object2: thhub |
2320 | + operation: disable # Never in collision |
2321 | + - object1: rfdistal |
2322 | + object2: thmiddle |
2323 | + operation: disable # Never in collision |
2324 | + - object1: rfdistal |
2325 | + object2: thproximal |
2326 | + operation: disable # Never in collision |
2327 | + - object1: rfdistal |
2328 | + object2: wrist |
2329 | + operation: disable # Never in collision |
2330 | + - object1: rfknuckle |
2331 | + object2: rfmiddle |
2332 | + operation: disable # Never in collision |
2333 | + - object1: rfknuckle |
2334 | + object2: shadowarm_base |
2335 | + operation: disable # Never in collision |
2336 | + - object1: rfknuckle |
2337 | + object2: shadowarm_handsupport |
2338 | + operation: disable # Never in collision |
2339 | + - object1: rfknuckle |
2340 | + object2: shadowarm_lowerarm |
2341 | + operation: disable # Never in collision |
2342 | + - object1: rfknuckle |
2343 | + object2: shadowarm_trunk |
2344 | + operation: disable # Never in collision |
2345 | + - object1: rfknuckle |
2346 | + object2: shadowarm_upperarm |
2347 | + operation: disable # Never in collision |
2348 | + - object1: rfknuckle |
2349 | + object2: thbase |
2350 | + operation: disable # Never in collision |
2351 | + - object1: rfknuckle |
2352 | + object2: thdistal |
2353 | + operation: disable # Never in collision |
2354 | + - object1: rfknuckle |
2355 | + object2: thhub |
2356 | + operation: disable # Never in collision |
2357 | + - object1: rfknuckle |
2358 | + object2: thmiddle |
2359 | + operation: disable # Never in collision |
2360 | + - object1: rfknuckle |
2361 | + object2: thproximal |
2362 | + operation: disable # Never in collision |
2363 | + - object1: rfknuckle |
2364 | + object2: wrist |
2365 | + operation: disable # Never in collision |
2366 | + - object1: rfmiddle |
2367 | + object2: shadowarm_base |
2368 | + operation: disable # Never in collision |
2369 | + - object1: rfmiddle |
2370 | + object2: shadowarm_handsupport |
2371 | + operation: disable # Never in collision |
2372 | + - object1: rfmiddle |
2373 | + object2: shadowarm_lowerarm |
2374 | + operation: disable # Never in collision |
2375 | + - object1: rfmiddle |
2376 | + object2: shadowarm_trunk |
2377 | + operation: disable # Never in collision |
2378 | + - object1: rfmiddle |
2379 | + object2: shadowarm_upperarm |
2380 | + operation: disable # Never in collision |
2381 | + - object1: rfmiddle |
2382 | + object2: thbase |
2383 | + operation: disable # Never in collision |
2384 | + - object1: rfmiddle |
2385 | + object2: thdistal |
2386 | + operation: disable # Never in collision |
2387 | + - object1: rfmiddle |
2388 | + object2: thhub |
2389 | + operation: disable # Never in collision |
2390 | + - object1: rfmiddle |
2391 | + object2: thmiddle |
2392 | + operation: disable # Never in collision |
2393 | + - object1: rfmiddle |
2394 | + object2: thproximal |
2395 | + operation: disable # Never in collision |
2396 | + - object1: rfmiddle |
2397 | + object2: wrist |
2398 | + operation: disable # Never in collision |
2399 | + - object1: rfproximal |
2400 | + object2: shadowarm_base |
2401 | + operation: disable # Never in collision |
2402 | + - object1: rfproximal |
2403 | + object2: shadowarm_handsupport |
2404 | + operation: disable # Never in collision |
2405 | + - object1: rfproximal |
2406 | + object2: shadowarm_lowerarm |
2407 | + operation: disable # Never in collision |
2408 | + - object1: rfproximal |
2409 | + object2: shadowarm_trunk |
2410 | + operation: disable # Never in collision |
2411 | + - object1: rfproximal |
2412 | + object2: shadowarm_upperarm |
2413 | + operation: disable # Never in collision |
2414 | + - object1: rfproximal |
2415 | + object2: thbase |
2416 | + operation: disable # Never in collision |
2417 | + - object1: rfproximal |
2418 | + object2: thdistal |
2419 | + operation: disable # Never in collision |
2420 | + - object1: rfproximal |
2421 | + object2: thhub |
2422 | + operation: disable # Never in collision |
2423 | + - object1: rfproximal |
2424 | + object2: thmiddle |
2425 | + operation: disable # Never in collision |
2426 | + - object1: rfproximal |
2427 | + object2: thproximal |
2428 | + operation: disable # Never in collision |
2429 | + - object1: rfproximal |
2430 | + object2: wrist |
2431 | + operation: disable # Never in collision |
2432 | + - object1: shadowarm_base |
2433 | + object2: shadowarm_handsupport |
2434 | + operation: disable # Never in collision |
2435 | + - object1: shadowarm_base |
2436 | + object2: shadowarm_lowerarm |
2437 | + operation: disable # Never in collision |
2438 | + - object1: shadowarm_base |
2439 | + object2: shadowarm_upperarm |
2440 | + operation: disable # Never in collision |
2441 | + - object1: shadowarm_base |
2442 | + object2: thbase |
2443 | + operation: disable # Never in collision |
2444 | + - object1: shadowarm_base |
2445 | + object2: thdistal |
2446 | + operation: disable # Never in collision |
2447 | + - object1: shadowarm_base |
2448 | + object2: thhub |
2449 | + operation: disable # Never in collision |
2450 | + - object1: shadowarm_base |
2451 | + object2: thmiddle |
2452 | + operation: disable # Never in collision |
2453 | + - object1: shadowarm_base |
2454 | + object2: thproximal |
2455 | + operation: disable # Never in collision |
2456 | + - object1: shadowarm_base |
2457 | + object2: wrist |
2458 | + operation: disable # Never in collision |
2459 | + - object1: shadowarm_handsupport |
2460 | + object2: shadowarm_trunk |
2461 | + operation: disable # Never in collision |
2462 | + - object1: shadowarm_handsupport |
2463 | + object2: shadowarm_upperarm |
2464 | + operation: disable # Never in collision |
2465 | + - object1: shadowarm_handsupport |
2466 | + object2: thbase |
2467 | + operation: disable # Never in collision |
2468 | + - object1: shadowarm_handsupport |
2469 | + object2: thdistal |
2470 | + operation: disable # Never in collision |
2471 | + - object1: shadowarm_handsupport |
2472 | + object2: thhub |
2473 | + operation: disable # Never in collision |
2474 | + - object1: shadowarm_handsupport |
2475 | + object2: thmiddle |
2476 | + operation: disable # Never in collision |
2477 | + - object1: shadowarm_handsupport |
2478 | + object2: thproximal |
2479 | + operation: disable # Never in collision |
2480 | + - object1: shadowarm_handsupport |
2481 | object2: wrist |
2482 | operation: disable # Never in collision |
2483 | - object1: shadowarm_lowerarm |
2484 | |
2485 | === modified file 'shadow_arm_navigation/launch/constraint_aware_kinematics.launch' |
2486 | --- shadow_arm_navigation/launch/constraint_aware_kinematics.launch 2012-04-18 19:58:04 +0000 |
2487 | +++ shadow_arm_navigation/launch/constraint_aware_kinematics.launch 2012-10-31 08:29:25 +0000 |
2488 | @@ -2,7 +2,7 @@ |
2489 | <include file="$(find shadow_arm_navigation)/launch/shadow_planning_environment.launch" /> |
2490 | <node pkg="arm_kinematics_constraint_aware" type="arm_kinematics_constraint_aware" name="shadow_right_arm_kinematics"> |
2491 | <param name="group" type="string" value="right_arm" /> |
2492 | - <param name="right_arm/root_name" type="string" value="desk_support" /> |
2493 | + <param name="right_arm/root_name" type="string" value="shadowarm_base" /> |
2494 | <param name="right_arm/tip_name" type="string" value="palm" /> |
2495 | <param name="kinematics_solver" type="string" value="arm_kinematics_constraint_aware/KDLArmKinematicsPlugin" /> |
2496 | <remap from="joint_states" to="/joint_states"/> |
2497 | |
2498 | === modified file 'shadow_arm_navigation/launch/shadow_arm_navigation.launch' |
2499 | --- shadow_arm_navigation/launch/shadow_arm_navigation.launch 2012-04-18 19:58:04 +0000 |
2500 | +++ shadow_arm_navigation/launch/shadow_arm_navigation.launch 2012-10-31 08:29:25 +0000 |
2501 | @@ -2,7 +2,7 @@ |
2502 | <include file="$(find shadow_arm_navigation)/launch/shadow_planning_environment.launch" /> |
2503 | <include file="$(find planning_environment)/launch/environment_server.launch"> |
2504 | <arg name="use_monitor" value="true" /> |
2505 | - <arg name="use_collision_map" value="true" /> |
2506 | + <arg name="use_collision_map" value="false" /> |
2507 | </include> |
2508 | <include file="$(find shadow_arm_navigation)/launch/constraint_aware_kinematics.launch" /> |
2509 | <include file="$(find shadow_arm_navigation)/launch/ompl_planning.launch" /> |
2510 | |
2511 | === modified file 'sr_gui_manipulation/lib/SrGuiManipulation.py' |
2512 | --- sr_gui_manipulation/lib/SrGuiManipulation.py 2012-08-28 08:10:49 +0000 |
2513 | +++ sr_gui_manipulation/lib/SrGuiManipulation.py 2012-10-31 08:29:25 +0000 |
2514 | @@ -17,6 +17,10 @@ |
2515 | import os |
2516 | import math |
2517 | |
2518 | +import scipy |
2519 | +import scipy.linalg |
2520 | +import numpy as np |
2521 | + |
2522 | import roslib; roslib.load_manifest('sr_gui_manipulation') |
2523 | import rospy |
2524 | from rospy import loginfo, logerr, logdebug |
2525 | @@ -32,9 +36,10 @@ |
2526 | from tabletop_object_detector.srv import TabletopDetection |
2527 | from tabletop_collision_map_processing.srv import TabletopCollisionMapProcessing |
2528 | from household_objects_database_msgs.srv import GetModelDescription |
2529 | +from household_objects_database_msgs.msg import DatabaseModelPose |
2530 | import object_manipulator.draw_functions as draw_functions |
2531 | from object_manipulation_msgs.srv import FindClusterBoundingBox, FindClusterBoundingBoxRequest |
2532 | -from object_manipulation_msgs.msg import Grasp, PickupGoal, PickupAction, PlaceGoal, PlaceAction |
2533 | +from object_manipulation_msgs.msg import Grasp, PickupGoal, PickupAction, PickupResult, PlaceGoal, PlaceAction, ManipulationResult, GraspableObject |
2534 | from object_manipulator.convert_functions import * |
2535 | from geometry_msgs.msg import Vector3Stamped, PoseStamped, Pose |
2536 | import actionlib |
2537 | @@ -44,6 +49,10 @@ |
2538 | from tf import transformations |
2539 | import tf |
2540 | |
2541 | +import copy |
2542 | + |
2543 | +from execution import Execution |
2544 | + |
2545 | import yaml |
2546 | |
2547 | class ObjectChooser(QWidget): |
2548 | @@ -61,6 +70,7 @@ |
2549 | self.pickup_result = None |
2550 | self.listener = tf.TransformListener() |
2551 | self.grasp_display_publisher = rospy.Publisher("/grasp_display", Grasp) |
2552 | + self.pickupservice = Execution() |
2553 | |
2554 | def draw(self): |
2555 | self.frame = QFrame(self) |
2556 | @@ -98,52 +108,95 @@ |
2557 | |
2558 | box_mat = pose_to_mat(box_pose.pose) |
2559 | rospy.logerr("box_pose %f %f %f q: %f %f %f %f",box_pose.pose.position.x,box_pose.pose.position.y,box_pose.pose.position.z, |
2560 | -box_pose.pose.orientation.x,box_pose.pose.orientation.y,box_pose.pose.orientation.z,box_pose.pose.orientation.w) |
2561 | + box_pose.pose.orientation.x,box_pose.pose.orientation.y,box_pose.pose.orientation.z,box_pose.pose.orientation.w) |
2562 | box_ranges = [[-box_dims.x / 2, -box_dims.y / 2, -box_dims.z / 2], |
2563 | [box_dims.x / 2, box_dims.y / 2, box_dims.z / 2]] |
2564 | self.box_pose=box_pose |
2565 | |
2566 | - self.draw_functions.draw_rviz_box(box_mat, box_ranges, '/fixed', |
2567 | + self.draw_functions.draw_rviz_box(box_mat, box_ranges, '/world', |
2568 | ns='bounding box', |
2569 | color=[0, 0, 1], opaque=0.25, duration=60) |
2570 | |
2571 | # call the pickup service |
2572 | res = self.pickup(graspable_object, self.object.graspable_object_name, self.object_name) |
2573 | - |
2574 | + |
2575 | if res == 0: #correctly picked up |
2576 | #TODO: set up place_location |
2577 | + executed_grasp = self.pickup_result.grasp |
2578 | initial_pose = PoseStamped() |
2579 | initial_pose.header.stamp = rospy.get_rostime() |
2580 | - initial_pose.header.frame_id = "/fixed" |
2581 | - initial_pose.pose.position.x = 0.0#self.box_pose.pose.position.x+0.0 |
2582 | - initial_pose.pose.position.y = -0.1#self.box_pose.pose.position.y-0.2 |
2583 | - initial_pose.pose.position.z = 0.05#self.box_pose.pose.position.z+0.05 |
2584 | + initial_pose.header.frame_id = "/world" |
2585 | + initial_pose.pose.position.x = self.box_pose.pose.position.x+0.1 |
2586 | + initial_pose.pose.position.y = self.box_pose.pose.position.y-0.3 |
2587 | + initial_pose.pose.position.z = self.box_pose.pose.position.z-box_dims.z/2 # graspable object is from bottom but bounding box is at center ! |
2588 | q=transformations.quaternion_about_axis(-0.05, (0,0,1)) |
2589 | - initial_pose.pose.orientation.x = q[0] |
2590 | - initial_pose.pose.orientation.y = q[1] |
2591 | - initial_pose.pose.orientation.z = q[2] |
2592 | - initial_pose.pose.orientation.w = q[3] |
2593 | + initial_pose.pose.orientation.x = self.box_pose.pose.orientation.x #q[0] |
2594 | + initial_pose.pose.orientation.y = self.box_pose.pose.orientation.y#q[1] |
2595 | + initial_pose.pose.orientation.z = self.box_pose.pose.orientation.z#q[2] |
2596 | + initial_pose.pose.orientation.w = self.box_pose.pose.orientation.w#q[3] |
2597 | |
2598 | - self.list_of_poses = self.compute_list_of_poses(initial_pose, graspable_object) |
2599 | + self.list_of_poses = self.compute_list_of_poses(initial_pose, graspable_object, executed_grasp) |
2600 | + #print "list of pose",self.list_of_poses |
2601 | |
2602 | self.place_object(graspable_object, self.object.graspable_object_name, self.object_name, self.list_of_poses) |
2603 | + |
2604 | |
2605 | def place_click(self): |
2606 | + |
2607 | + if (self.pickup_result): |
2608 | + grasp = self.pickup_result.grasp |
2609 | + self.pickupservice.pre_grasp_exec(grasp,10.0) |
2610 | + else: |
2611 | + self.pickupservice.grasp_release_exec(10.0) |
2612 | + |
2613 | initial_pose = PoseStamped() |
2614 | initial_pose.header.stamp = rospy.get_rostime() |
2615 | - initial_pose.header.frame_id = self.object.graspable_object.reference_frame_id#"/fixed" |
2616 | - initial_pose.pose.position.x = 0.2#self.box_pose.pose.position.x+0.0 |
2617 | - initial_pose.pose.position.y = -0.1#self.box_pose.pose.position.y-0.2 |
2618 | - initial_pose.pose.position.z = 0.05#self.box_pose.pose.position.z+0.05 |
2619 | + initial_pose.header.frame_id = "/world"#"/fixed" |
2620 | + |
2621 | + #fake hand to world pose |
2622 | + initial_pose.pose.position.x = 0.4#self.box_pose.pose.position.x+0.0 |
2623 | + initial_pose.pose.position.y = 0.091# self.box_pose.pose.position.y-0.2 |
2624 | + initial_pose.pose.position.z = 1.25#self.box_pose.pose.position.z+0.05 |
2625 | q=transformations.quaternion_about_axis(-0.05, (0,0,1)) |
2626 | - initial_pose.pose.orientation.x = 0#q[0] |
2627 | - initial_pose.pose.orientation.y = 0#q[1] |
2628 | - initial_pose.pose.orientation.z = 0#q[2] |
2629 | - initial_pose.pose.orientation.w = 1#q[3] |
2630 | - |
2631 | - self.list_of_poses = self.compute_list_of_poses(initial_pose, self.object.graspable_object) |
2632 | - |
2633 | - self.place_object(self.object.graspable_object, self.object.graspable_object_name, self.object_name, self.list_of_poses) |
2634 | + initial_pose.pose.orientation.x = 0.41#self.box_pose.pose.orientation.x#q[0] |
2635 | + initial_pose.pose.orientation.y = 0.695#self.box_pose.pose.orientation.y#q[1] |
2636 | + initial_pose.pose.orientation.z = 0.52#self.box_pose.pose.orientation.z#q[2] |
2637 | + initial_pose.pose.orientation.w = 0.284#self.box_pose.pose.orientation.w#q[3] |
2638 | + |
2639 | + executed_grasp=Grasp() |
2640 | + executed_grasp.grasp_pose=copy.deepcopy(initial_pose.pose) |
2641 | + |
2642 | + #fake obj world origin pose |
2643 | + initial_pose.pose.position.x = 0.43 |
2644 | + initial_pose.pose.position.y = 0.149 |
2645 | + initial_pose.pose.position.z = 1.088 |
2646 | + initial_pose.pose.orientation.x = 0.0#self.box_pose.pose.orientation.x#q[0] |
2647 | + initial_pose.pose.orientation.y = 0.0#self.box_pose.pose.orientation.y#q[1] |
2648 | + initial_pose.pose.orientation.z = 0.567#self.box_pose.pose.orientation.z#q[2] |
2649 | + initial_pose.pose.orientation.w = 0.824#self.box_pose.pose.orientation.w#q[3] |
2650 | + |
2651 | + #fake graspable object |
2652 | + graspable_object = GraspableObject() |
2653 | + graspable_object.reference_frame_id = "/world" |
2654 | + mypotentialmodels = DatabaseModelPose() |
2655 | + mypotentialmodels.model_id = 18744 |
2656 | + mypotentialmodels.confidence = 1.0 |
2657 | + mypotentialmodels.pose = copy.deepcopy(initial_pose) |
2658 | + graspable_object.potential_models.append(mypotentialmodels) |
2659 | + |
2660 | + #fake obj world destination pose |
2661 | + initial_pose.pose.position.x = 0.43 |
2662 | + initial_pose.pose.position.y = 0.0 |
2663 | + initial_pose.pose.position.z = 1.088 |
2664 | + initial_pose.pose.orientation.x = 0.0#self.box_pose.pose.orientation.x#q[0] |
2665 | + initial_pose.pose.orientation.y = 0.0#self.box_pose.pose.orientation.y#q[1] |
2666 | + initial_pose.pose.orientation.z = 0.567#self.box_pose.pose.orientation.z#q[2] |
2667 | + initial_pose.pose.orientation.w = 0.824#self.box_pose.pose.orientation.w#q[3] |
2668 | + |
2669 | + list_of_poses = self.compute_list_of_poses(initial_pose, graspable_object, executed_grasp) |
2670 | + #print "list of pose",list_of_poses |
2671 | + |
2672 | + #self.place_object(self.object.graspable_object, self.object.graspable_object_name, self.object_name, self.list_of_poses) |
2673 | |
2674 | def pickup(self, graspable_object, graspable_object_name, object_name): |
2675 | """ |
2676 | @@ -158,11 +211,12 @@ |
2677 | pickup_goal.collision_object_name = graspable_object_name |
2678 | pickup_goal.collision_support_surface_name = self.gui.collision_support_surface_name |
2679 | |
2680 | + #pickup_goal.additional_link_padding |
2681 | pickup_goal.ignore_collisions = True |
2682 | |
2683 | pickup_goal.arm_name = "right_arm" |
2684 | - #pickup_goal.desired_approach_distance = 0.05 |
2685 | - #pickup_goal.min_approach_distance = 0.02 |
2686 | + #pickup_goal.desired_approach_distance = 0.08 This does not exist anymore in the message |
2687 | + #pickup_goal.min_approach_distance = 0.02 This does not exist anymore in the message |
2688 | |
2689 | direction = Vector3Stamped() |
2690 | direction.header.stamp = rospy.get_rostime() |
2691 | @@ -178,17 +232,20 @@ |
2692 | pickup_goal.use_reactive_lift = True; |
2693 | pickup_goal.use_reactive_execution = True; |
2694 | |
2695 | - pickup_client = actionlib.SimpleActionClient('/object_manipulator/object_manipulator_pickup', PickupAction) |
2696 | - pickup_client.wait_for_server() |
2697 | - rospy.loginfo("Pickup server ready") |
2698 | + self.pickup_result = self.pickupservice.pick(pickup_goal) |
2699 | + |
2700 | + #pickup_client = actionlib.SimpleActionClient('/object_manipulator/object_manipulator_pickup', PickupAction) |
2701 | + #pickup_client.wait_for_server() |
2702 | + #rospy.loginfo("Pickup server ready") |
2703 | |
2704 | - pickup_client.send_goal(pickup_goal) |
2705 | + #pickup_client.send_goal(pickup_goal) |
2706 | #TODO: change this when using the robot |
2707 | - pickup_client.wait_for_result(timeout=rospy.Duration.from_sec(3600.0)) |
2708 | + #pickup_client.wait_for_result(timeout=rospy.Duration.from_sec(3600.0)) |
2709 | loginfo("Got Pickup results") |
2710 | - self.pickup_result = pickup_client.get_result() |
2711 | + #self.pickup_result = pickup_client.get_result() |
2712 | |
2713 | #print "Pickup result: "+str(self.pickup_result) |
2714 | + ''' |
2715 | if pickup_client.get_state() != GoalStatus.SUCCEEDED: |
2716 | rospy.logerr("The pickup action has failed: " + str(self.pickup_result.manipulation_result.value) ) |
2717 | QMessageBox.warning(self, "Warning", |
2718 | @@ -197,6 +254,15 @@ |
2719 | if tested_grasp_result.result_code==7: |
2720 | self.grasp_display_publisher.publish(tested_grasp) |
2721 | return -1 |
2722 | + ''' |
2723 | + |
2724 | + if self.pickup_result.manipulation_result.value == ManipulationResult.SUCCESS: |
2725 | + loginfo("Pick succeeded, now lifting") |
2726 | + self.pickupservice.lift(0.07) |
2727 | + |
2728 | + else: |
2729 | + loginfo("Pick failed") |
2730 | + return 1 |
2731 | return 0 |
2732 | |
2733 | def place_object(self, graspable_object, graspable_object_name, object_name, list_of_poses ): |
2734 | @@ -233,20 +299,22 @@ |
2735 | #which is along the z axis in the fixed frame |
2736 | direction = Vector3Stamped() |
2737 | direction.header.stamp = rospy.get_rostime() |
2738 | - direction.header.frame_id = "/base_link" |
2739 | + direction.header.frame_id = "/world" |
2740 | direction.vector.x = 0 |
2741 | direction.vector.y = 0 |
2742 | direction.vector.z = 1 |
2743 | place_goal.approach.direction = direction |
2744 | - place_goal.approach.min_distance = 0.02 |
2745 | - place_goal.approach.desired_distance = 0.1 |
2746 | + place_goal.approach.min_distance = 0.01 |
2747 | + place_goal.approach.desired_distance = 0.03 |
2748 | #request a vertical put down motion of 10cm before placing the object |
2749 | place_goal.desired_retreat_distance = 0.1 |
2750 | place_goal.min_retreat_distance = 0.05 |
2751 | #we are not using tactile based placing |
2752 | place_goal.use_reactive_place = False |
2753 | |
2754 | - place_client = actionlib.SimpleActionClient('/object_manipulator/object_manipulator_place', PlaceAction) |
2755 | + |
2756 | + placeresult = self.pickupservice.place(place_goal) |
2757 | + '''place_client = actionlib.SimpleActionClient('/object_manipulator/object_manipulator_place', PlaceAction) |
2758 | place_client.wait_for_server() |
2759 | rospy.loginfo("Place server ready") |
2760 | |
2761 | @@ -261,38 +329,83 @@ |
2762 | if place_client.get_state() != GoalStatus.SUCCEEDED: |
2763 | rospy.logerr("The place action has failed: " + str(place_result.manipulation_result.value) ) |
2764 | print place_result |
2765 | + ''' |
2766 | + |
2767 | + if placeresult.manipulation_result.value == ManipulationResult.SUCCESS: |
2768 | + loginfo("Place succeeded, now retreating") |
2769 | + self.pickupservice.retreat(0.07) |
2770 | + else: |
2771 | + loginfo("Place failed") |
2772 | + return 1 |
2773 | + return 0 |
2774 | + |
2775 | |
2776 | - def compute_list_of_poses(self, initial_pose, graspable_object, rect_w=0.10, rect_h=0.10, resolution=0.02): |
2777 | - ''' |
2778 | - Computes a list of possible poses in a rectangle of 2*rect_w by 2*rect_h, with the given resolution. |
2779 | - In our case, rect_w is along the x axis, and rect_h along the y_axis. |
2780 | - ''' |
2781 | + def compute_list_of_poses(self, destination_pose, graspable_object, executed_grasp): |
2782 | + |
2783 | list_of_poses = [] |
2784 | - |
2785 | - current_pose = PoseStamped() |
2786 | - current_pose.header.stamp = rospy.get_rostime() |
2787 | - current_pose.header.frame_id = initial_pose.header.frame_id #"/fixed" |
2788 | - current_pose.pose.position.x = initial_pose.pose.position.x |
2789 | - current_pose.pose.position.y = initial_pose.pose.position.y |
2790 | - current_pose.pose.position.z = initial_pose.pose.position.z |
2791 | - current_pose.pose.orientation.x = initial_pose.pose.orientation.x |
2792 | - current_pose.pose.orientation.y = initial_pose.pose.orientation.y |
2793 | - current_pose.pose.orientation.z = initial_pose.pose.orientation.z |
2794 | - current_pose.pose.orientation.w = initial_pose.pose.orientation.w |
2795 | - rospy.loginfo( " placing object at: "+str(current_pose) ) |
2796 | - |
2797 | - list_of_poses.append(current_pose) |
2798 | + |
2799 | + # find hand to object transform |
2800 | + # executed_grasp is hand pose in world frame |
2801 | + # graspable_object is object in world frame |
2802 | + hand_world_pose = executed_grasp.grasp_pose |
2803 | + #print "hand_world", hand_world_pose |
2804 | + object_world_pose = graspable_object.potential_models[0].pose.pose |
2805 | + # print "object world" , object_world_pose |
2806 | + tmathandworld = pose_to_mat(hand_world_pose) |
2807 | + tmatobjworld = pose_to_mat(object_world_pose) |
2808 | + hand_object_pose = mat_to_pose(np.dot(np.linalg.inv(tmatobjworld),tmathandworld)) |
2809 | + #print "hand_object_pose" , hand_object_pose |
2810 | + |
2811 | + # generate rotational_symmetric hand to object frames |
2812 | + hand_object_poses = self.generate_rotational_symmetric_poses(hand_object_pose,16) |
2813 | + |
2814 | + target_pose = PoseStamped() |
2815 | + target_pose=copy.deepcopy(destination_pose) |
2816 | + target_pose.header.stamp = rospy.get_rostime() |
2817 | + #print "target_object_world pose" , target_pose |
2818 | + tmattarget_world = pose_to_mat(target_pose.pose) |
2819 | + |
2820 | + for hand_object_pose in hand_object_poses: |
2821 | + tmathandobj = pose_to_mat(hand_object_pose) |
2822 | + target_hand_posestamped_world = PoseStamped() |
2823 | + target_hand_posestamped_world.header.frame_id = "/world" |
2824 | + target_hand_posestamped_world.pose= mat_to_pose(np.dot(tmattarget_world,tmathandobj)) |
2825 | + target_hand_posestamped_world.header.stamp = rospy.get_rostime() |
2826 | + list_of_poses.append(target_hand_posestamped_world) |
2827 | |
2828 | self.draw_place_area(list_of_poses, graspable_object) |
2829 | |
2830 | return list_of_poses |
2831 | + |
2832 | + def generate_rotational_symmetric_poses(self, hand_object_pose,step=8): |
2833 | + ''' |
2834 | + Generate a list of 20 pregrasps around the z axis of the object |
2835 | + ''' |
2836 | + generated_poses = [] |
2837 | + pose=hand_object_pose |
2838 | + #We'll generate 20 poses centered in 0,0,0 with orientations in 360 degrees around z |
2839 | + |
2840 | + for i in range(0,step): |
2841 | + angle = (6.28318532 * i)/step |
2842 | + q = tf.transformations.quaternion_from_euler(0.0,0.0,angle) |
2843 | + rotation_pose = Pose(Point(0,0,0), Quaternion(0.0,0.0,0.0,1.0)) |
2844 | + rotation_pose.orientation = Quaternion(*q) |
2845 | + |
2846 | + tmatrotz = pose_to_mat(rotation_pose) |
2847 | + # premultiply by the rotation matrix |
2848 | + tmatpose = pose_to_mat(pose) |
2849 | + rotated_pose = mat_to_pose(np.dot(tmatrotz,tmatpose)) |
2850 | + |
2851 | + generated_poses.append(rotated_pose) |
2852 | + |
2853 | + return generated_poses |
2854 | |
2855 | def draw_place_area(self, list_of_poses, graspable_object): |
2856 | ''' |
2857 | Displays all the possible placing locations that are going to be tried. |
2858 | ''' |
2859 | #try: |
2860 | - (trans_palm,rot_palm) = self.listener.lookupTransform('/fixed', '/palm', rospy.Time(0)) |
2861 | + (trans_palm,rot_palm) = self.listener.lookupTransform('/world', '/palm', rospy.Time(0)) |
2862 | #except: |
2863 | # return |
2864 | |
2865 | @@ -308,7 +421,7 @@ |
2866 | pose_tmp.orientation.w = pose_stamped.pose.orientation.w |
2867 | |
2868 | mat = pose_to_mat(pose_tmp) |
2869 | - self.draw_functions.draw_rviz_box(mat, [.01,.01,.01], frame='/object_frame', ns='place_'+str(index), |
2870 | + self.draw_functions.draw_rviz_box(mat, [.01,.01,.01], frame='/world', ns='place_'+str(index), |
2871 | id=1000+index, duration = 90, color=[0.5,0.5,0.0], opaque=1.0 ) |
2872 | |
2873 | def call_find_cluster_bounding_box(self, cluster): |
2874 | @@ -413,8 +526,8 @@ |
2875 | self.win.btn_collision_map.pressed.connect(self.process_collision_map) |
2876 | self.win.btn_collision_map.setEnabled(False) |
2877 | self.win.btn_start_grab_position.pressed.connect(self.start_grab_position) |
2878 | - self.win.btn_zero_position.pressed.connect(self.zero_position) |
2879 | -# self.win.btn_zero_position.pressed.connect(self.redo_place) |
2880 | +# self.win.btn_zero_position.pressed.connect(self.zero_position) |
2881 | + self.win.btn_zero_position.pressed.connect(self.redo_place) |
2882 | self.robot_lib_eth = EtherCAT_Hand_Lib() |
2883 | |
2884 | # Guillaume: Currently removed because requires ethercat to be active to run, ethercat does exist in sim |
2885 | @@ -486,7 +599,7 @@ |
2886 | except rospy.ServiceException, e: |
2887 | print "Service did not process request: %s" % str(e) |
2888 | |
2889 | - print self.raw_objects |
2890 | + #print self.raw_objects |
2891 | # Take a new collision map + add the detected objects to the collision |
2892 | # map and get graspable objects from them |
2893 | tabletop_collision_map_res = self.process_collision_map() |
2894 | @@ -516,7 +629,7 @@ |
2895 | res = 0 |
2896 | try: |
2897 | #Args: detection_result reset_collision_models reset_attached_models desired_frame |
2898 | - res = self.service_tabletop_collision_map.call(self.raw_objects.detection, True, True, "/fixed") |
2899 | + res = self.service_tabletop_collision_map.call(self.raw_objects.detection, True, True, "/world") |
2900 | except rospy.ServiceException, e: |
2901 | logerr("Service did not process request: %s" % str(e)) |
2902 | |
2903 | @@ -652,6 +765,7 @@ |
2904 | |
2905 | def start_grab_position(self): |
2906 | """Move the arm into a good starting position for the grab""" |
2907 | - self.position_arm('start_grab') |
2908 | + #self.position_arm('start_grab') |
2909 | + |
2910 | |
2911 | |
2912 | |
2913 | === modified file 'sr_gui_manipulation/manifest.xml' |
2914 | --- sr_gui_manipulation/manifest.xml 2012-09-03 13:10:43 +0000 |
2915 | +++ sr_gui_manipulation/manifest.xml 2012-10-31 08:29:25 +0000 |
2916 | @@ -21,6 +21,7 @@ |
2917 | <depend package="sr_hand"/> |
2918 | <depend package="actionlib"/> |
2919 | <depend package="control_msgs"/> |
2920 | + <depend package="sr_pick_and_place"/> |
2921 | |
2922 | <export> |
2923 | <rosgui plugin="${prefix}/sr_gui_manipulation.xml"/> |
2924 | |
2925 | === modified file 'sr_object_manipulation_launch/config/gazebo_tactile_thresholds.yaml' |
2926 | --- sr_object_manipulation_launch/config/gazebo_tactile_thresholds.yaml 2012-07-27 20:16:07 +0000 |
2927 | +++ sr_object_manipulation_launch/config/gazebo_tactile_thresholds.yaml 2012-10-31 08:29:25 +0000 |
2928 | @@ -6,14 +6,14 @@ |
2929 | # fingers will close around the object to lift it. |
2930 | |
2931 | light_touch_thresholds: |
2932 | - - 1.0 |
2933 | - - 0.0 |
2934 | - - 0.0 |
2935 | - - 0.0 |
2936 | - - 1.0 |
2937 | + - 0.0 #0.05 |
2938 | + - 0.0 |
2939 | + - 0.0 |
2940 | + - 0.0 |
2941 | + - 0.0 #0.05 |
2942 | grasp_touch_thresholds: |
2943 | - - 1.2 |
2944 | - - 0.0 |
2945 | - - 0.0 |
2946 | - - 0.0 |
2947 | - - 1.2 |
2948 | + - 0.0 #0.1 |
2949 | + - 0.0 |
2950 | + - 0.0 |
2951 | + - 0.0 |
2952 | + - 0.0 #0.1 |
2953 | |
2954 | === modified file 'sr_object_manipulation_launch/config/hand_description.yaml' |
2955 | --- sr_object_manipulation_launch/config/hand_description.yaml 2012-04-09 13:10:43 +0000 |
2956 | +++ sr_object_manipulation_launch/config/hand_description.yaml 2012-10-31 08:29:25 +0000 |
2957 | @@ -9,7 +9,7 @@ |
2958 | hand_database_name: SHADOW_HAND |
2959 | hand_joints: [FFJ4,FFJ3,FFJ0,MFJ4,MFJ3,MFJ0,RFJ4,RFJ3,RFJ0,LFJ5,LFJ4,LFJ3,LFJ0,THJ5,THJ4,THJ3,THJ2,THJ1] |
2960 | arm_joints: [ShoulderJRotate,ShoulderJSwing,ElbowJSwing,ElbowJRotate,WRJ2,WRJ1] |
2961 | - hand_touch_links: [ffdistal,mfdistal,rfdistal,lfdistal,thdistal,ffmiddle,mfmiddle,rfmiddle,lfmiddle,thmiddle,ffproximal,mfproximal,rfproximal,lfproximal,thproximal,palm] |
2962 | + hand_touch_links: [ffdistal,mfdistal,rfdistal,lfdistal,thdistal,ffmiddle,mfmiddle,rfmiddle,lfmiddle,thmiddle,ffproximal,mfproximal,rfproximal,lfproximal,lfmetacarpal,thbase,thproximal,palm] |
2963 | |
2964 | hand_fingertip_links: [ffdistal,mfdistal,rfdistal,lfdistal,thdistal] |
2965 | hand_approach_direction: [0.0,-1.0,0.0] |
2966 | |
2967 | === modified file 'sr_object_manipulation_launch/config/household_object_server.yaml' |
2968 | --- sr_object_manipulation_launch/config/household_object_server.yaml 2012-08-28 09:56:27 +0000 |
2969 | +++ sr_object_manipulation_launch/config/household_object_server.yaml 2012-10-31 08:29:25 +0000 |
2970 | @@ -1,6 +1,6 @@ |
2971 | household_objects_database: |
2972 | - database_host: ugocupcic.dyndns.org |
2973 | + database_host: bachelet |
2974 | database_port: 5432 |
2975 | database_user: willow |
2976 | database_pass: willow |
2977 | - database_name: household_objects_database |
2978 | + database_name: household_objects_electric |
2979 | |
2980 | === modified file 'sr_object_manipulation_launch/config/self_filter.yaml' |
2981 | --- sr_object_manipulation_launch/config/self_filter.yaml 2012-07-25 20:54:34 +0000 |
2982 | +++ sr_object_manipulation_launch/config/self_filter.yaml 2012-10-31 08:29:25 +0000 |
2983 | @@ -42,7 +42,7 @@ |
2984 | padding: .04 |
2985 | - name: shadowarm_base |
2986 | padding: .04 |
2987 | - - name: shadow_handsupport |
2988 | + - name: shadowarm_handsupport |
2989 | padding: .04 |
2990 | - name: shadowarm_lowerarm |
2991 | padding: .04 |
2992 | |
2993 | === modified file 'sr_object_manipulation_launch/launch/arm_and_hand_motor.launch' |
2994 | --- sr_object_manipulation_launch/launch/arm_and_hand_motor.launch 2012-08-28 09:56:27 +0000 |
2995 | +++ sr_object_manipulation_launch/launch/arm_and_hand_motor.launch 2012-10-31 08:29:25 +0000 |
2996 | @@ -54,7 +54,7 @@ |
2997 | <node pkg="sr_tactile_sensors" name="sr_tactile" type="sr_tactile_virtual_gazebo" respawn="false"> |
2998 | <rosparam file="$(find sr_tactile_sensors)/params/sensor_names.yaml" command="load"/> |
2999 | </node> |
3000 | - <node pkg="tf" type="static_transform_publisher" name="fixed_frame_pub_arm" args="0 0 0 0 0 0 fixed /desk_support 100" /> |
3001 | +<!-- <node pkg="tf" type="static_transform_publisher" name="fixed_frame_pub_arm" args="0 0 0 0 0 0 fixed /base_link 100" /> --> |
3002 | </group> |
3003 | </launch> |
3004 | |
3005 | |
3006 | === modified file 'sr_object_manipulation_launch/launch/collision_map.launch' |
3007 | --- sr_object_manipulation_launch/launch/collision_map.launch 2012-08-22 16:26:42 +0000 |
3008 | +++ sr_object_manipulation_launch/launch/collision_map.launch 2012-10-31 08:29:25 +0000 |
3009 | @@ -36,7 +36,7 @@ |
3010 | <!-- we do not want a separate map with occlusions alone --> |
3011 | <param name="publish_occlusion" type="bool" value="true" /> |
3012 | |
3013 | - <param name="fixed_frame" type="string" value="/fixed" /> |
3014 | + <param name="fixed_frame" type="string" value="/world" /> |
3015 | |
3016 | <!-- define a box of size 0.5x0.5x0.5 around (0, 0, 0) in the robot frame |
3017 | --> |
3018 | |
3019 | === modified file 'sr_object_manipulation_launch/launch/kinect_static_transform.launch' |
3020 | --- sr_object_manipulation_launch/launch/kinect_static_transform.launch 2012-05-04 21:38:54 +0000 |
3021 | +++ sr_object_manipulation_launch/launch/kinect_static_transform.launch 2012-10-31 08:29:25 +0000 |
3022 | @@ -3,8 +3,10 @@ |
3023 | <!-- This transform should be computed by the calibration software --> |
3024 | <!-- <node pkg="tf" type="static_transform_publisher" name="trunk_to_kinect" |
3025 | args="0.027 0.205 0.422248 -0.594811 0.36128 0.203393 0.688727 /shadowarm_trunk /camera_link 100" machine="threedsensor"/> --> |
3026 | - <node pkg="tf" type="static_transform_publisher" name="trunk_to_kinect" |
3027 | - args="0.0094 0.200 0.385 -0.235 0.81 -1.57533 /shadowarm_trunk /camera_link 100" machine="threedsensor"/> |
3028 | +<!-- before arm change <node pkg="tf" type="static_transform_publisher" name="trunk_to_kinect" |
3029 | + args="0.0094 0.200 0.385 -0.235 0.81 -1.57533 /shadowarm_trunk /camera_link 100" machine="threedsensor"/> --> |
3030 | +<node pkg="tf" type="static_transform_publisher" name="trunk_to_kinect" |
3031 | + args="0.00930639 0.16721 0.366541 -0.598431 0.354668 0.177556 0.696107 /shadowarm_trunk /camera_link 100" machine="threedsensor"/> |
3032 | |
3033 | <!-- new panned 45 deg <node pkg="tf" type="static_transform_publisher" name="trunk_to_kinect" |
3034 | args="-0.11404 0.140594 0.387348 -0.686704 0.0976917 0.450266 0.562276 /shadowarm_trunk /camera_link 100" machine="threedsensor"/> --> |
3035 | |
3036 | === modified file 'sr_object_manipulation_launch/launch/manipulator.launch' |
3037 | --- sr_object_manipulation_launch/launch/manipulator.launch 2012-05-01 16:46:59 +0000 |
3038 | +++ sr_object_manipulation_launch/launch/manipulator.launch 2012-10-31 08:29:25 +0000 |
3039 | @@ -15,7 +15,7 @@ |
3040 | |
3041 | <node name="sr_grasp_planner" pkg="sr_grasp_planner" type="sr_grasp_planner" respawn="false"/> |
3042 | |
3043 | - <node name="object_manipulator" pkg="object_manipulator" type="object_manipulator_node" |
3044 | +<!-- <node name="object_manipulator" pkg="object_manipulator" type="object_manipulator_node" |
3045 | respawn="false" |
3046 | output="screen"> |
3047 | <remap from="right_arm/constraint_aware_ik" to="/shadow_right_arm_kinematics/get_constraint_aware_ik" /> |
3048 | @@ -23,9 +23,9 @@ |
3049 | <remap from="right_arm/interpolated_ik_set_params" to="/r_interpolated_ik_motion_plan_set_params"/> |
3050 | <remap from="right_arm/get_fk" to="/shadow_right_arm_kinematics/get_fk" /> |
3051 | <remap from="right_arm/get_ik_solver_info" to="/shadow_right_arm_kinematics/get_ik_solver_info"/> |
3052 | - |
3053 | +--> |
3054 | <!--<remap from="right_arm_joint_controller" to="/shadow_right_arm_kinematics/get_ik_solver_info"/> --> |
3055 | - <param name="right_arm_joint_controller" value="r_arm_joint_trajectory_controller" /> |
3056 | +<!-- <param name="right_arm_joint_controller" value="r_arm_joint_trajectory_controller" /> |
3057 | |
3058 | <param name="right_arm_cartesian_controller" value="r_arm_cartesian_pose_controller" /> |
3059 | |
3060 | @@ -44,5 +44,5 @@ |
3061 | <remap from="/list_controllers" to="/sr_controller_manager/list_controllers"/> |
3062 | <remap from="/switch_controller" to="/sr_controller_manager/switch_controller"/> |
3063 | </node> |
3064 | - |
3065 | +--> |
3066 | </launch> |
3067 | |
3068 | === modified file 'sr_object_manipulation_launch/launch/static_transform.launch' |
3069 | --- sr_object_manipulation_launch/launch/static_transform.launch 2012-08-22 16:26:42 +0000 |
3070 | +++ sr_object_manipulation_launch/launch/static_transform.launch 2012-10-31 08:29:25 +0000 |
3071 | @@ -1,9 +1,7 @@ |
3072 | <launch> |
3073 | - <node pkg="tf" type="static_transform_publisher" name="world_to_desk_support" |
3074 | - args="0.0 0.0 0.0 0.0 0.0 0.0 /world /desk_support 100"/> |
3075 | <node pkg="tf" type="static_transform_publisher" name="world_to_base_link" |
3076 | args="0.0 0.0 0.0 0.0 0.0 0.0 /world /base_link 100"/> |
3077 | |
3078 | - <node pkg="tf" type="static_transform_publisher" name="world_to_fixed" |
3079 | - args="0.0 0.0 0.0 0.0 0.0 0.0 /world /fixed 100"/> |
3080 | +<!-- <node pkg="tf" type="static_transform_publisher" name="world_to_fixed" |
3081 | + args="0.0 0.0 0.0 0.0 0.0 0.0 /world /fixed 100"/> --> |
3082 | </launch> |
3083 | |
3084 | === modified file 'sr_object_manipulation_launch/launch/tabletop_node.launch' |
3085 | --- sr_object_manipulation_launch/launch/tabletop_node.launch 2012-04-17 19:22:56 +0000 |
3086 | +++ sr_object_manipulation_launch/launch/tabletop_node.launch 2012-10-31 08:29:25 +0000 |
3087 | @@ -43,7 +43,7 @@ |
3088 | <!--database parameters--> |
3089 | <!--NOTE: this launch file does not start the database wrapper node--> |
3090 | <!--that must be done externally--> |
3091 | - <param name="model_set" value="REDUCED_MODEL_SET" /> |
3092 | + <param name="model_set" value="COKECAN_SET" /> |
3093 | <param name="get_model_list_srv" value="/objects_database_node/get_model_list" /> |
3094 | <param name="get_model_mesh_srv" value="/objects_database_node/get_model_mesh" /> |
3095 | </node> |
3096 | |
3097 | === added directory 'sr_pick_and_place' |
3098 | === added file 'sr_pick_and_place/CMakeLists.txt' |
3099 | --- sr_pick_and_place/CMakeLists.txt 1970-01-01 00:00:00 +0000 |
3100 | +++ sr_pick_and_place/CMakeLists.txt 2012-10-31 08:29:25 +0000 |
3101 | @@ -0,0 +1,30 @@ |
3102 | +cmake_minimum_required(VERSION 2.4.6) |
3103 | +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) |
3104 | + |
3105 | +# Set the build type. Options are: |
3106 | +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage |
3107 | +# Debug : w/ debug symbols, w/o optimization |
3108 | +# Release : w/o debug symbols, w/ optimization |
3109 | +# RelWithDebInfo : w/ debug symbols, w/ optimization |
3110 | +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries |
3111 | +#set(ROS_BUILD_TYPE RelWithDebInfo) |
3112 | + |
3113 | +rosbuild_init() |
3114 | + |
3115 | +#set the default path for built executables to the "bin" directory |
3116 | +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) |
3117 | +#set the default path for built libraries to the "lib" directory |
3118 | +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) |
3119 | + |
3120 | +#uncomment if you have defined messages |
3121 | +#rosbuild_genmsg() |
3122 | +#uncomment if you have defined services |
3123 | +#rosbuild_gensrv() |
3124 | + |
3125 | +#common commands for building c++ executables and libraries |
3126 | +#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) |
3127 | +#target_link_libraries(${PROJECT_NAME} another_library) |
3128 | +#rosbuild_add_boost_directories() |
3129 | +#rosbuild_link_boost(${PROJECT_NAME} thread) |
3130 | +#rosbuild_add_executable(example examples/example.cpp) |
3131 | +#target_link_libraries(example ${PROJECT_NAME}) |
3132 | |
3133 | === added file 'sr_pick_and_place/Makefile' |
3134 | --- sr_pick_and_place/Makefile 1970-01-01 00:00:00 +0000 |
3135 | +++ sr_pick_and_place/Makefile 2012-10-31 08:29:25 +0000 |
3136 | @@ -0,0 +1,1 @@ |
3137 | +include $(shell rospack find mk)/cmake.mk |
3138 | \ No newline at end of file |
3139 | |
3140 | === added file 'sr_pick_and_place/mainpage.dox' |
3141 | --- sr_pick_and_place/mainpage.dox 1970-01-01 00:00:00 +0000 |
3142 | +++ sr_pick_and_place/mainpage.dox 2012-10-31 08:29:25 +0000 |
3143 | @@ -0,0 +1,14 @@ |
3144 | +/** |
3145 | +\mainpage |
3146 | +\htmlinclude manifest.html |
3147 | + |
3148 | +\b sr_pick_and_place |
3149 | + |
3150 | +<!-- |
3151 | +Provide an overview of your package. |
3152 | +--> |
3153 | + |
3154 | +--> |
3155 | + |
3156 | + |
3157 | +*/ |
3158 | |
3159 | === added file 'sr_pick_and_place/manifest.xml' |
3160 | --- sr_pick_and_place/manifest.xml 1970-01-01 00:00:00 +0000 |
3161 | +++ sr_pick_and_place/manifest.xml 2012-10-31 08:29:25 +0000 |
3162 | @@ -0,0 +1,21 @@ |
3163 | +<package> |
3164 | + <description brief="sr_pick_and_place"> |
3165 | + |
3166 | + sr_pick_and_place |
3167 | + |
3168 | + </description> |
3169 | + <author>Ugo Cupcic</author> |
3170 | + <license>BSD</license> |
3171 | + <review status="unreviewed" notes=""/> |
3172 | + <url>http://ros.org/wiki/sr_pick_and_place</url> |
3173 | + <depend package="rospy"/> |
3174 | + <depend package="actionlib"/> |
3175 | + <depend package="arm_navigation_msgs"/> |
3176 | + <depend package="geometry_msgs"/> |
3177 | + <depend package="kinematics_msgs"/> |
3178 | + <depend package="trajectory_msgs"/> |
3179 | + <depend package="sr_utilities"/> |
3180 | + |
3181 | +</package> |
3182 | + |
3183 | + |
3184 | |
3185 | === added directory 'sr_pick_and_place/src' |
3186 | === added file 'sr_pick_and_place/src/execution.py' |
3187 | --- sr_pick_and_place/src/execution.py 1970-01-01 00:00:00 +0000 |
3188 | +++ sr_pick_and_place/src/execution.py 2012-10-31 08:29:25 +0000 |
3189 | @@ -0,0 +1,601 @@ |
3190 | +#!/usr/bin/env python |
3191 | +# |
3192 | +# Copyright 2011 Shadow Robot Company Ltd. |
3193 | +# |
3194 | +# This program is free software: you can redistribute it and/or modify it |
3195 | +# under the terms of the GNU General Public License as published by the Free |
3196 | +# Software Foundation, either version 2 of the License, or (at your option) |
3197 | +# any later version. |
3198 | +# |
3199 | +# This program is distributed in the hope that it will be useful, but WITHOUT |
3200 | +# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or |
3201 | +# FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for |
3202 | +# more details. |
3203 | +# |
3204 | +# You should have received a copy of the GNU General Public License along |
3205 | +# with this program. If not, see <http://www.gnu.org/licenses/>. |
3206 | +# |
3207 | + |
3208 | +import roslib; roslib.load_manifest("sr_pick_and_place") |
3209 | +import rospy |
3210 | + |
3211 | +from object_manipulation_msgs.msg import Grasp, PickupGoal, PickupResult, PickupAction, PlaceGoal, PlaceResult, PlaceAction, ManipulationResult |
3212 | +from object_manipulation_msgs.srv import GraspPlanning, GraspPlanningRequest, GraspPlanningResponse |
3213 | +from object_manipulation_msgs.msg import GraspHandPostureExecutionAction ,GraspHandPostureExecutionGoal |
3214 | +from arm_navigation_msgs.srv import GetMotionPlanRequest, GetMotionPlanResponse, FilterJointTrajectory, FilterJointTrajectoryRequest |
3215 | +from arm_navigation_msgs.msg import DisplayTrajectory, JointLimits, AttachedCollisionObject, CollisionObjectOperation, Shape |
3216 | +from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal |
3217 | +from geometry_msgs.msg import PoseStamped, Point,Quaternion,Pose |
3218 | +from actionlib_msgs.msg import GoalID, GoalStatus, GoalStatusArray |
3219 | +from trajectory_msgs.msg import JointTrajectory |
3220 | +from sr_utilities.srv import getJointState |
3221 | +from planification import Planification |
3222 | +from sr_utilities.srv import getJointState |
3223 | +import actionlib |
3224 | + |
3225 | +from tf import transformations |
3226 | +import tf |
3227 | + |
3228 | +import time |
3229 | +import copy |
3230 | + |
3231 | +ARM_NAMES = ['ShoulderJRotate', 'ShoulderJSwing', 'ElbowJSwing', 'ElbowJRotate', "WRJ1", "WRJ2"] |
3232 | + |
3233 | +class Execution(object): |
3234 | + """ |
3235 | + """ |
3236 | + |
3237 | + def __init__(self, ): |
3238 | + """ |
3239 | + """ |
3240 | + |
3241 | + self.simdelay = 10.0 #3.0 #10.0 for simulation , 2 or less for real |
3242 | + #initialize the planner |
3243 | + self.plan = Planification() |
3244 | + |
3245 | + self.display_traj_pub_ = rospy.Publisher("/joint_path_display", DisplayTrajectory, latch=True) |
3246 | + self.send_traj_pub_ = rospy.Publisher("/command", JointTrajectory, latch=True) |
3247 | + self.att_object_in_map_pub_= rospy.Publisher("/attached_collision_object", AttachedCollisionObject,latch=True) |
3248 | + |
3249 | + |
3250 | + rospy.loginfo("Waiting for services /getJointState, /trajectory_filter_unnormalizer/filter_trajectory, /database_grasp_planning") |
3251 | + rospy.wait_for_service("/getJointState") |
3252 | + rospy.wait_for_service("/trajectory_filter_unnormalizer/filter_trajectory") |
3253 | + rospy.wait_for_service("/objects_database_node/database_grasp_planning") |
3254 | + rospy.loginfo(" OK services found") |
3255 | + |
3256 | + self.get_joint_state_ = rospy.ServiceProxy("/getJointState", getJointState) |
3257 | + # TODO use another getJointstateProvided by env Server |
3258 | + self.trajectory_filter_ = rospy.ServiceProxy("/trajectory_filter_unnormalizer/filter_trajectory", FilterJointTrajectory) |
3259 | + self.grasp_planning_service_ = rospy.ServiceProxy("/objects_database_node/database_grasp_planning", GraspPlanning) |
3260 | + # access hand_posture execution actionlib |
3261 | + self.hand_posture_execution_actionclient_ = actionlib.SimpleActionClient('/right_arm/hand_posture_execution', GraspHandPostureExecutionAction) |
3262 | + self.hand_posture_execution_actionclient_.wait_for_server() |
3263 | + rospy.loginfo("hand_posture_execution server ready") |
3264 | + |
3265 | + # access arm_movement actionlib |
3266 | + self.joint_spline_trajectory_actionclient_ = actionlib.SimpleActionClient('/r_arm_controller/joint_trajectory_action', FollowJointTrajectoryAction) |
3267 | + self.joint_spline_trajectory_actionclient_.wait_for_server() |
3268 | + rospy.loginfo("joint_spline_trajectory server ready") |
3269 | + |
3270 | + |
3271 | + |
3272 | + self.listener = tf.TransformListener() |
3273 | + |
3274 | + def pick(self,pickup_goal): |
3275 | + #prepare result |
3276 | + pickresult = PickupResult() |
3277 | + |
3278 | + #get grasps for the object |
3279 | + # fill up a grasp planning request |
3280 | + grasp_planning_req = GraspPlanningRequest() |
3281 | + grasp_planning_req.arm_name = pickup_goal.arm_name |
3282 | + grasp_planning_req.target = pickup_goal.target |
3283 | + object_to_attach = pickup_goal.collision_object_name |
3284 | + # call grasp planning service |
3285 | + grasp_planning_res = self.grasp_planning_service_.call(grasp_planning_req) |
3286 | + #print grasp_planning_res |
3287 | + # process grasp planning result |
3288 | + if (grasp_planning_res.error_code.value != grasp_planning_res.error_code.SUCCESS): |
3289 | + rospy.logerr("No grasp found for this object, we will generate some, but only when the node is ready for that !") |
3290 | + pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE |
3291 | + return pickresult |
3292 | + else: |
3293 | + rospy.loginfo("Got "+ str(len(grasp_planning_res.grasps)) +" grasps for this object") |
3294 | + |
3295 | + # for each grasp, generate rotational symmetric grasps around the object (this is already in the DB for the CokeCan but should be removed and done online) |
3296 | + |
3297 | + #for each grasp, check path from pre-grasp pose to grasp pose first and then check motion to pre-grasp pose |
3298 | + motion_plan_res=GetMotionPlanResponse() |
3299 | + grasp_to_execute_=Grasp() |
3300 | + for index, grasp in enumerate(grasp_planning_res.grasps): |
3301 | + # extract grasp_pose |
3302 | + grasp_pose_ = PoseStamped() |
3303 | + grasp_pose_.header.frame_id = "/world"; |
3304 | + grasp_pose_.pose = grasp.grasp_pose |
3305 | + |
3306 | + grasp_pose_.pose.position.y=grasp_pose_.pose.position.y-0.01 #-0.01 in sim, +0.03 in real #cheating here |
3307 | + |
3308 | + # copy the grasp_pose as a pre-grasp_pose |
3309 | + pre_grasp_pose_ = copy.deepcopy(grasp_pose_) |
3310 | + |
3311 | + # add desired_approach_distance along the approach vector. above the object to plan pre-grasp pose |
3312 | + |
3313 | + # currently add this to Z because approach vector needs to be computed somehow first (TODO) |
3314 | + pre_grasp_pose_.pose.position.z = pre_grasp_pose_.pose.position.z + 0.05 |
3315 | + |
3316 | + # for distance from 0 (grasp_pose) to desired_approach distance (pre_grasp_pose) test IK/Collision and save result |
3317 | + # decompose this in X steps depending on distance to do and max speed |
3318 | + interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(pre_grasp_pose_, grasp_pose_, False) |
3319 | + |
3320 | + # check the result (depending on number of steps etc...) |
3321 | + if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS): |
3322 | + number_of_interpolated_steps=0 |
3323 | + # check if one approach trajectory is feasible |
3324 | + for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes): |
3325 | + if traj_error_code.val!=1: |
3326 | + rospy.logerr("One unfeasible approach-phase step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val)) |
3327 | + break |
3328 | + else: |
3329 | + number_of_interpolated_steps=interpolation_index |
3330 | + |
3331 | + # if trajectory is feasible then plan reach motion to pre-grasp pose |
3332 | + if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points): |
3333 | + rospy.loginfo("Grasp number "+str(index)+" approach is possible, checking motion plan to pre-grasp") |
3334 | + #print interpolated_motion_plan_res |
3335 | + |
3336 | + # check and plan motion to this pre_grasp_pose |
3337 | + motion_plan_res = self.plan.plan_arm_motion( pickup_goal.arm_name, "jointspace", pre_grasp_pose_ ) |
3338 | + |
3339 | + #if this pre-grasp pose is successful do not test others |
3340 | + if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS): |
3341 | + rospy.loginfo("Grasp number "+str(index)+" is possible, executing it") |
3342 | + # copy the grasp to execute for the following steps |
3343 | + grasp_to_execute_ = copy.deepcopy(grasp) |
3344 | + break |
3345 | + else: |
3346 | + rospy.logerr("Grasp number "+str(index)+" approach is impossible") |
3347 | + #print interpolated_motion_plan_res |
3348 | + else: |
3349 | + rospy.logerr("Grasp number "+str(index)+" approach is impossible") |
3350 | + #print interpolated_motion_plan_res |
3351 | + # execution part |
3352 | + if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS): |
3353 | + #put hand in pre-grasp posture |
3354 | + if self.pre_grasp_exec(grasp_to_execute_)<0: |
3355 | + #QMessageBox.warning(self, "Warning", |
3356 | + # "Pre-grasp action failed: ") |
3357 | + pickresult.manipulation_result.value = ManipulationResult.FAILED |
3358 | + return pickresult |
3359 | + |
3360 | + #go there |
3361 | + # filter the trajectory |
3362 | + filtered_traj = self.filter_traj_(motion_plan_res) |
3363 | + |
3364 | + self.display_traj_( filtered_traj ) |
3365 | + |
3366 | + # reach pregrasp pose |
3367 | + if self.send_traj_( filtered_traj )<0: |
3368 | + #QMessageBox.warning(self, "Warning", |
3369 | + # "Reach trajectory execution failed: ") |
3370 | + pickresult.manipulation_result.value = ManipulationResult.FAILED |
3371 | + return pickresult |
3372 | + #time.sleep(20) # TODO use actionlib here |
3373 | + time.sleep(self.simdelay) # TODO use actionlib here |
3374 | + |
3375 | + # approach |
3376 | + if self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory )<0: |
3377 | + #QMessageBox.warning(self, "Warning", |
3378 | + # "Approach trajectory execution failed: ") |
3379 | + pickresult.manipulation_result.value = ManipulationResult.FAILED |
3380 | + return pickresult |
3381 | + time.sleep(self.simdelay) # TODO use actionlib here |
3382 | + |
3383 | + #grasp |
3384 | + if self.grasp_exec(grasp_to_execute_)<0: |
3385 | + #QMessageBox.warning(self, "Warning", |
3386 | + # "Grasp action failed: ") |
3387 | + pickresult.manipulation_result.value = ManipulationResult.FAILED |
3388 | + return pickresult |
3389 | + time.sleep(self.simdelay) # TODO use actionlib here |
3390 | + |
3391 | + #attach the collision object to the hand (should be cleaned-up) |
3392 | + rospy.loginfo("Now we attach the object") |
3393 | + |
3394 | + att_object = AttachedCollisionObject() |
3395 | + att_object.link_name = "palm" |
3396 | + att_object.object.id = object_to_attach |
3397 | + att_object.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT |
3398 | + att_object.object.header.frame_id = "palm" |
3399 | + att_object.object.header.stamp = rospy.Time.now() |
3400 | + object = Shape() |
3401 | + object.type = Shape.CYLINDER |
3402 | + object.dimensions.append(.03) |
3403 | + object.dimensions.append(0.1) |
3404 | + pose = Pose() |
3405 | + pose.position.x = 0.0 |
3406 | + pose.position.y = -0.06 |
3407 | + pose.position.z = 0.06 |
3408 | + pose.orientation.x = 0 |
3409 | + pose.orientation.y = 0 |
3410 | + pose.orientation.z = 0 |
3411 | + pose.orientation.w = 1 |
3412 | + att_object.object.shapes.append(object) |
3413 | + att_object.object.poses.append(pose); |
3414 | + att_object.touch_links= ["ffdistal","mfdistal","rfdistal","lfdistal","thdistal","ffmiddle","mfmiddle","rfmiddle","lfmiddle","thmiddle","ffproximal","mfproximal","rfproximal","lfproximal","thproximal","palm","lfmetacarpal","thbase"] |
3415 | + self.att_object_in_map_pub_.publish(att_object) |
3416 | + rospy.loginfo("Attach object published") |
3417 | + else: |
3418 | + rospy.logerr("None of the grasps tested is possible") |
3419 | + pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE |
3420 | + return pickresult |
3421 | + pickresult.manipulation_result.value = ManipulationResult.SUCCESS |
3422 | + pickresult.grasp= grasp_to_execute_ |
3423 | + return pickresult |
3424 | + |
3425 | + def lift(self, target_lift_distance): |
3426 | + #straight movement above object, just for compatibility |
3427 | + |
3428 | + #check if object is grasped |
3429 | + |
3430 | + # get current hand pose |
3431 | + self.listener.waitForTransform('/world', '/palm', rospy.Time(), rospy.Duration(1.0)) |
3432 | + try: |
3433 | + (trans,rot) = self.listener.lookupTransform('/world', '/palm', rospy.Time()) |
3434 | + except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): |
3435 | + rospy.logerr("Cannot get current palm pose") |
3436 | + return |
3437 | + |
3438 | + current_pose_= PoseStamped() |
3439 | + current_pose_.header.frame_id = "/world" |
3440 | + current_pose_.pose.position = Point(trans[0],trans[1],trans[2]) |
3441 | + current_pose_.pose.orientation = Quaternion(rot[0],rot[1],rot[2],rot[3]) |
3442 | + |
3443 | + # prepare target pose |
3444 | + target_pose_=PoseStamped() |
3445 | + target_pose_.header.frame_id = "/world" |
3446 | + target_pose_.pose.position = Point(trans[0],trans[1],trans[2]+target_lift_distance) |
3447 | + target_pose_.pose.orientation = Quaternion(rot[0],rot[1],rot[2],rot[3]) #keep same orientation for the first test |
3448 | + |
3449 | + #compute straight trajectory in 6D or in 3D if it fails |
3450 | + # for distance from 0 (grasp_pose) to desired_height test IK/Collision and save result |
3451 | + # decompose this in X steps depending on distance to do and max speed |
3452 | + interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(current_pose_, target_pose_, False) |
3453 | + |
3454 | + # check the result (depending on number of steps etc...) |
3455 | + if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS): |
3456 | + number_of_interpolated_steps=0 |
3457 | + # check if one approach trajectory is feasible |
3458 | + for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes): |
3459 | + if traj_error_code.val!=1: |
3460 | + rospy.logerr("One unfeasible lift step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val)) |
3461 | + break |
3462 | + else: |
3463 | + number_of_interpolated_steps=interpolation_index |
3464 | + |
3465 | + if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points): |
3466 | + rospy.loginfo("lift is possible") |
3467 | + else: |
3468 | + rospy.logerr("lift is impossible") |
3469 | + print interpolated_motion_plan_res |
3470 | + return |
3471 | + else: |
3472 | + rospy.logerr("lift planning failed") |
3473 | + print interpolated_motion_plan_res |
3474 | + return |
3475 | + |
3476 | + self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory ) |
3477 | + time.sleep(self.simdelay) # TODO use actionlib here |
3478 | + return |
3479 | + |
3480 | + def place(self,place_goal): |
3481 | + |
3482 | + placeresult = PlaceResult() |
3483 | + target_pose_to_execute_ = PoseStamped() |
3484 | + #for location, check path from approach pose to release pose first and then check motion to approach pose |
3485 | + motion_plan_res=GetMotionPlanResponse() |
3486 | + object_to_attach = place_goal.collision_object_name |
3487 | + # get current hand pose |
3488 | + self.listener.waitForTransform('/world', '/palm', rospy.Time(), rospy.Duration(1.0)) |
3489 | + try: |
3490 | + (trans,rot) = self.listener.lookupTransform('/world', '/palm', rospy.Time()) |
3491 | + except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): |
3492 | + rospy.logerr("Cannot get current palm pose") |
3493 | + placeresult.manipulation_result.value = ManipulationResult.ERROR |
3494 | + return placeresult |
3495 | + |
3496 | + current_pose_= PoseStamped() |
3497 | + current_pose_.header.frame_id = "/world" |
3498 | + current_pose_.pose.position = Point(trans[0],trans[1],trans[2]) |
3499 | + current_pose_.pose.orientation = Quaternion(rot[0],rot[1],rot[2],rot[3]) |
3500 | + |
3501 | + # for each place location |
3502 | + for index, target_pose_ in enumerate(place_goal.place_locations): |
3503 | + |
3504 | + #compute straight trajectory to approach distance |
3505 | + target_approach_pose_= PoseStamped() |
3506 | + target_approach_pose_.header.frame_id = "/world" |
3507 | + target_approach_pose_.pose.position = Point(target_pose_.pose.position.x,target_pose_.pose.position.y,target_pose_.pose.position.z+place_goal.approach.desired_distance) |
3508 | + target_approach_pose_.pose.orientation = Quaternion(target_pose_.pose.orientation.x,target_pose_.pose.orientation.y,target_pose_.pose.orientation.z,target_pose_.pose.orientation.w) #keep same orientation for the first test |
3509 | + |
3510 | + # for distance from 0 (release_pose) to desired_approach distance (approach_pose) test IK/Collision and save result |
3511 | + # decompose this in X steps depending on distance to do and max speed |
3512 | + interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(target_approach_pose_, target_pose_, False) |
3513 | + |
3514 | + # check the result (depending on number of steps etc...) |
3515 | + if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS): |
3516 | + number_of_interpolated_steps=0 |
3517 | + # check if one approach trajectory is feasible |
3518 | + for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes): |
3519 | + if traj_error_code.val!=1: |
3520 | + rospy.logerr("One unfeasible approach-phase step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val)) |
3521 | + break |
3522 | + else: |
3523 | + number_of_interpolated_steps=interpolation_index |
3524 | + |
3525 | + # if trajectory is feasible then plan reach motion to approach pose |
3526 | + if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points): |
3527 | + rospy.loginfo("Place pose number "+str(index)+" approach is possible, checking motion plan to approach pose") |
3528 | + #print interpolated_motion_plan_res |
3529 | + |
3530 | + # check and plan motion to this approach pose |
3531 | + motion_plan_res = self.plan.plan_arm_motion( place_goal.arm_name, "jointspace" ,target_approach_pose_)#,object_to_attach) |
3532 | + |
3533 | + #if this approach pose is successful do not test others |
3534 | + if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS): |
3535 | + rospy.loginfo("Place pose number "+str(index)+" is possible, executing it") |
3536 | + # copy the pose to execute for the following steps |
3537 | + target_pose_to_execute_ = copy.deepcopy(target_pose_) |
3538 | + break |
3539 | + else: |
3540 | + rospy.logerr("Place pose number "+str(index)+" approach is impossible") |
3541 | + #print interpolated_motion_plan_res |
3542 | + else: |
3543 | + rospy.logerr("Place pose number "+str(index)+" approach is impossible") |
3544 | + #print interpolated_motion_plan_res |
3545 | + |
3546 | + # execution part |
3547 | + if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS): |
3548 | + #go there |
3549 | + # filter the trajectory |
3550 | + filtered_traj = self.filter_traj_(motion_plan_res) |
3551 | + |
3552 | + self.display_traj_( filtered_traj ) |
3553 | + |
3554 | + # reach approach pose |
3555 | + if self.send_traj_( filtered_traj )<0: |
3556 | + #QMessageBox.warning(self, "Warning", |
3557 | + # "Reach trajectory execution failed: ") |
3558 | + placeresult.manipulation_result.value = ManipulationResult.FAILED |
3559 | + return placeresult |
3560 | + time.sleep(self.simdelay) # TODO use actionlib here |
3561 | + |
3562 | + # approach |
3563 | + if self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory )<0: |
3564 | + #QMessageBox.warning(self, "Warning", |
3565 | + # "Approach trajectory execution failed: ") |
3566 | + placeresult.manipulation_result.value = ManipulationResult.FAILED |
3567 | + return placeresult |
3568 | + time.sleep(self.simdelay) # TODO use actionlib here |
3569 | + |
3570 | + #put hand in pre-grasp posture (to gently release) |
3571 | + if self.pre_grasp_exec(place_goal.grasp)<0: |
3572 | + #QMessageBox.warning(self, "Warning", |
3573 | + # "Release action failed: ") |
3574 | + placeresult.manipulation_result.value = ManipulationResult.FAILED |
3575 | + return placeresult |
3576 | + time.sleep(self.simdelay) # TODO use actionlib here |
3577 | + #detach the object from the hand |
3578 | + rospy.loginfo("Now we detach the attached object") |
3579 | + att_object = AttachedCollisionObject() |
3580 | + att_object.link_name = "palm" |
3581 | + att_object.object.id = object_to_attach |
3582 | + att_object.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT |
3583 | + att_object.object.header.frame_id = "palm" |
3584 | + att_object.object.header.stamp = rospy.Time.now() |
3585 | + object = Shape() |
3586 | + object.type = Shape.CYLINDER |
3587 | + object.dimensions.append(.03) |
3588 | + object.dimensions.append(0.1) |
3589 | + pose = Pose() |
3590 | + pose.position.x = 0.0 |
3591 | + pose.position.y = -0.06 |
3592 | + pose.position.z = 0.06 |
3593 | + pose.orientation.x = 0 |
3594 | + pose.orientation.y = 0 |
3595 | + pose.orientation.z = 0 |
3596 | + pose.orientation.w = 1 |
3597 | + att_object.object.shapes.append(object) |
3598 | + att_object.object.poses.append(pose); |
3599 | + self.att_object_in_map_pub_.publish(att_object) |
3600 | + rospy.loginfo("Attached object to be detached published") |
3601 | + |
3602 | + |
3603 | + else: |
3604 | + rospy.logerr("None of the place pose tested is possible") |
3605 | + placeresult.manipulation_result.value = ManipulationResult.UNFEASIBLE |
3606 | + return placeresult |
3607 | + placeresult.manipulation_result.value = ManipulationResult.SUCCESS |
3608 | + placeresult.place_location= target_pose_to_execute_ |
3609 | + return placeresult |
3610 | + |
3611 | + def retreat(self, target_retreat_distance): |
3612 | + #straight movement above object, just for compatibility |
3613 | + |
3614 | + #check if object is released |
3615 | + # TODO |
3616 | + |
3617 | + # get current hand pose |
3618 | + self.listener.waitForTransform('/world', '/palm', rospy.Time(), rospy.Duration(1.0)) |
3619 | + try: |
3620 | + (trans,rot) = self.listener.lookupTransform('/world', '/palm', rospy.Time()) |
3621 | + except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): |
3622 | + rospy.logerr("Cannot get current palm pose") |
3623 | + return |
3624 | + |
3625 | + current_pose_= PoseStamped() |
3626 | + current_pose_.header.frame_id = "/world" |
3627 | + current_pose_.pose.position = Point(trans[0],trans[1],trans[2]) |
3628 | + current_pose_.pose.orientation = Quaternion(rot[0],rot[1],rot[2],rot[3]) |
3629 | + |
3630 | + # prepare target pose |
3631 | + target_pose_=PoseStamped() |
3632 | + target_pose_.header.frame_id = "/world" |
3633 | + target_pose_.pose.position = Point(trans[0],trans[1],trans[2]+target_retreat_distance) |
3634 | + target_pose_.pose.orientation = Quaternion(rot[0],rot[1],rot[2],rot[3]) #keep same orientation for the first test |
3635 | + |
3636 | + #compute straight trajectory in 6D or in 3D if it fails |
3637 | + # for distance from 0 (grasp_pose) to desired_height test IK/Collision and save result |
3638 | + # decompose this in X steps depending on distance to do and max speed |
3639 | + interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(current_pose_, target_pose_, False) |
3640 | + |
3641 | + # check the result (depending on number of steps etc...) |
3642 | + if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS): |
3643 | + number_of_interpolated_steps=0 |
3644 | + # check if one approach trajectory is feasible |
3645 | + for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes): |
3646 | + if traj_error_code.val!=1: |
3647 | + rospy.logerr("One unfeasible retreat step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val)) |
3648 | + break |
3649 | + else: |
3650 | + number_of_interpolated_steps=interpolation_index |
3651 | + |
3652 | + if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points): |
3653 | + rospy.loginfo("retreat is possible") |
3654 | + else: |
3655 | + rospy.logerr("retreat is impossible") |
3656 | + print interpolated_motion_plan_res |
3657 | + return |
3658 | + else: |
3659 | + rospy.logerr("retreat planning failed") |
3660 | + print interpolated_motion_plan_res |
3661 | + return |
3662 | + |
3663 | + self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory ) |
3664 | + time.sleep(self.simdelay) # TODO use actionlib here |
3665 | + return |
3666 | + |
3667 | + def grasp_release_exec(self,timeout_sec=10.0): |
3668 | + hand_posture_goal_ = GraspHandPostureExecutionGoal() |
3669 | + hand_posture_goal_.grasp = Grasp() |
3670 | + hand_posture_goal_.grasp.pre_grasp_posture.name= [ "FFJ0", "FFJ3", "FFJ4", "LFJ0", "LFJ3", "LFJ4", "LFJ5", "MFJ0", "MFJ3", "MFJ4", "RFJ0", "RFJ3", "RFJ4", "THJ1", "THJ2", "THJ3", "THJ4", "THJ5", "WRJ1", "WRJ2"] |
3671 | + hand_posture_goal_.grasp.pre_grasp_posture.position = [0]*(18) |
3672 | + # do release action |
3673 | + hand_posture_goal_.goal = GraspHandPostureExecutionGoal.RELEASE |
3674 | + # call hand_posture_exec action lib |
3675 | + rospy.loginfo("Trying to release...") |
3676 | + return self.hand_posture_exec(hand_posture_goal_,timeout_sec) |
3677 | + |
3678 | + def pre_grasp_exec(self, grasp,timeout_sec=10.0): |
3679 | + |
3680 | + hand_posture_goal_ = GraspHandPostureExecutionGoal() |
3681 | + hand_posture_goal_.grasp = grasp |
3682 | + # do pre-grasp action |
3683 | + hand_posture_goal_.goal = GraspHandPostureExecutionGoal.PRE_GRASP |
3684 | + hand_posture_goal_.max_contact_force = 0.0 |
3685 | + # call hand_posture_exec action lib |
3686 | + rospy.loginfo("Trying to pre-grasp...") |
3687 | + return self.hand_posture_exec(hand_posture_goal_,timeout_sec) |
3688 | + |
3689 | + def grasp_exec(self, grasp, timeout_sec=15.0): |
3690 | + |
3691 | + hand_posture_goal_ = GraspHandPostureExecutionGoal() |
3692 | + hand_posture_goal_.grasp = grasp |
3693 | + # do grasp action |
3694 | + hand_posture_goal_.goal = GraspHandPostureExecutionGoal.GRASP |
3695 | + hand_posture_goal_.max_contact_force = 1.0 |
3696 | + # call hand_posture_exec action lib |
3697 | + rospy.loginfo("Trying to grasp...") |
3698 | + return self.hand_posture_exec(hand_posture_goal_,timeout_sec) |
3699 | + |
3700 | + def hand_posture_exec(self, hand_posture_goal,timeout_sec): |
3701 | + # call hand_posture_exec action lib |
3702 | + self.hand_posture_execution_actionclient_.send_goal(hand_posture_goal) |
3703 | + # wait for result up to 10 seconds |
3704 | + self.hand_posture_execution_actionclient_.wait_for_result(timeout=rospy.Duration.from_sec(timeout_sec)) |
3705 | + #rospy.loginfo("Got Hand Posture Exec results") |
3706 | + # analyze the result |
3707 | + hand_posture_exec_result_ = self.hand_posture_execution_actionclient_.get_result() |
3708 | + if self.hand_posture_execution_actionclient_.get_state() != GoalStatus.SUCCEEDED: |
3709 | + rospy.logerr("The action has failed: " + str(hand_posture_exec_result_.result) ) |
3710 | + return -1 |
3711 | + else: |
3712 | + rospy.loginfo("The action has succeeded") |
3713 | + return 0 |
3714 | + |
3715 | + def display_traj_(self, trajectory): |
3716 | + print "Display trajectory" |
3717 | + |
3718 | + traj = DisplayTrajectory() |
3719 | + traj.model_id = "shadow" |
3720 | + traj.trajectory.joint_trajectory = trajectory |
3721 | + traj.trajectory.joint_trajectory.header.frame_id = "world" |
3722 | + traj.trajectory.joint_trajectory.header.stamp = rospy.Time.now() |
3723 | + self.display_traj_pub_.publish(traj) |
3724 | + |
3725 | + print " -> trajectory published" |
3726 | + time.sleep(0.5) |
3727 | + |
3728 | + def filter_traj_(self, motion_plan_res): |
3729 | + try: |
3730 | + req = FilterJointTrajectoryRequest() |
3731 | + for name in ARM_NAMES: |
3732 | + limit = JointLimits() |
3733 | + limit.joint_name = name |
3734 | + limit.min_position = -1.5 |
3735 | + limit.max_position = 1.5 |
3736 | + limit.has_velocity_limits = True |
3737 | + limit.max_velocity = 0.1 |
3738 | + limit.has_acceleration_limits = True |
3739 | + limit.max_acceleration = 0.1 |
3740 | + req.limits.append(limit) |
3741 | + |
3742 | + req.trajectory = motion_plan_res.trajectory.joint_trajectory |
3743 | + req.allowed_time = rospy.Duration.from_sec( 5.0 ) |
3744 | + |
3745 | + res = self.get_joint_state_.call() |
3746 | + req.start_state.joint_state = res.joint_state |
3747 | + |
3748 | + res = self.trajectory_filter_.call( req ) |
3749 | + except rospy.ServiceException, e: |
3750 | + rospy.logerr("Failed to filter "+str(e)) |
3751 | + return motion_plan_res.trajectory |
3752 | + |
3753 | + return res.trajectory |
3754 | + |
3755 | + def send_traj_(self, trajectory): |
3756 | + print "Sending trajectory" |
3757 | + #prepare goal |
3758 | + trajgoal = FollowJointTrajectoryGoal() |
3759 | + trajgoal.trajectory = trajectory |
3760 | + # send goal |
3761 | + self.joint_spline_trajectory_actionclient_.send_goal(trajgoal) |
3762 | + # wait for result up to 30 seconds |
3763 | + self.joint_spline_trajectory_actionclient_.wait_for_result(timeout=rospy.Duration.from_sec(50)) |
3764 | + # analyze result |
3765 | + joint_spline_trajectory_result_ = self.joint_spline_trajectory_actionclient_.get_result() |
3766 | + if self.joint_spline_trajectory_actionclient_.get_state() != GoalStatus.SUCCEEDED: |
3767 | + rospy.logerr("The joint_trajectory action has failed: " + str(joint_spline_trajectory_result_.error_code) ) |
3768 | + return -1 |
3769 | + else: |
3770 | + rospy.loginfo("The joint_trajectory action has succeeded") |
3771 | + return 0 |
3772 | + |
3773 | +if __name__ =="__main__": |
3774 | + rospy.init_node("execution") |
3775 | + execute = Execution() |
3776 | + |
3777 | + object_pose = PoseStamped() |
3778 | + object_pose.pose.position.x = 0.470 |
3779 | + object_pose.pose.position.y = 0.166 |
3780 | + object_pose.pose.position.z = 1.665 |
3781 | + |
3782 | + object_pose.pose.orientation.x = 0.375 |
3783 | + object_pose.pose.orientation.y = 0.155 |
3784 | + object_pose.pose.orientation.z = 0.844 |
3785 | + object_pose.pose.orientation.w = 0.351 |
3786 | + |
3787 | + execute.pick( object_pose ) |
3788 | + rospy.spin() |
3789 | + |
3790 | + |
3791 | |
3792 | === added file 'sr_pick_and_place/src/planification.py' |
3793 | --- sr_pick_and_place/src/planification.py 1970-01-01 00:00:00 +0000 |
3794 | +++ sr_pick_and_place/src/planification.py 2012-10-31 08:29:25 +0000 |
3795 | @@ -0,0 +1,437 @@ |
3796 | +#!/usr/bin/env python |
3797 | +# |
3798 | +# Copyright 2011 Shadow Robot Company Ltd. |
3799 | +# |
3800 | +# This program is free software: you can redistribute it and/or modify it |
3801 | +# under the terms of the GNU General Public License as published by the Free |
3802 | +# Software Foundation, either version 2 of the License, or (at your option) |
3803 | +# any later version. |
3804 | +# |
3805 | +# This program is distributed in the hope that it will be useful, but WITHOUT |
3806 | +# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or |
3807 | +# FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for |
3808 | +# more details. |
3809 | +# |
3810 | +# You should have received a copy of the GNU General Public License along |
3811 | +# with this program. If not, see <http://www.gnu.org/licenses/>. |
3812 | +# |
3813 | + |
3814 | +import roslib; roslib.load_manifest("sr_pick_and_place") |
3815 | +import rospy |
3816 | + |
3817 | +from arm_navigation_msgs.srv import GetMotionPlan, GetMotionPlanRequest, GetMotionPlanResponse, FilterJointTrajectoryWithConstraints, FilterJointTrajectoryWithConstraintsRequest |
3818 | +from arm_navigation_msgs.srv import SetPlanningSceneDiff, SetPlanningSceneDiffRequest |
3819 | +from arm_navigation_msgs.msg import MotionPlanRequest, PositionConstraint, OrientationConstraint, DisplayTrajectory, Constraints, JointConstraint, JointLimits, RobotState, AttachedCollisionObject, CollisionObjectOperation, Shape |
3820 | +from interpolated_ik_motion_planner.srv import SetInterpolatedIKMotionPlanParams |
3821 | +#import interpolated_ik_motion_planner.ik_utilities as ik_utilities |
3822 | +from kinematics_msgs.srv import GetConstraintAwarePositionIK |
3823 | +from kinematics_msgs.msg import PositionIKRequest |
3824 | +from geometry_msgs.msg import PoseStamped, Pose |
3825 | +from trajectory_msgs.msg import JointTrajectory |
3826 | +from sr_utilities.srv import getJointState |
3827 | + |
3828 | +import time |
3829 | +import math |
3830 | + |
3831 | +ARM_NAMES = ['ShoulderJRotate', 'ShoulderJSwing', 'ElbowJSwing', 'ElbowJRotate', "WRJ1", "WRJ2"] |
3832 | +ARM_IK_SEED = [-0.0011556513918362654, 0.33071140061761284, 1.9152039367468952, 0.008440334101951663, 0.0, 0.0] |
3833 | + |
3834 | +class Planification(object): |
3835 | + """ |
3836 | + """ |
3837 | + |
3838 | + def __init__(self, ): |
3839 | + """ |
3840 | + """ |
3841 | + rospy.loginfo("Waiting for services /ompl_planning/plan_kinematic_path, /environment_server/set_planning_scene_diff, /shadow_right_arm_kinematics/get_constraint_aware_ik ...") |
3842 | + |
3843 | + rospy.wait_for_service("/ompl_planning/plan_kinematic_path") |
3844 | + rospy.wait_for_service("/environment_server/set_planning_scene_diff") |
3845 | + rospy.wait_for_service("/shadow_right_arm_kinematics/get_constraint_aware_ik") |
3846 | + rospy.wait_for_service("/r_interpolated_ik_motion_plan_set_params") |
3847 | + rospy.wait_for_service("/r_interpolated_ik_motion_plan") |
3848 | + rospy.wait_for_service("/trajectory_filter_server/filter_trajectory_with_constraints") |
3849 | + rospy.wait_for_service("/getJointState") |
3850 | + rospy.loginfo(" OK services found") |
3851 | + |
3852 | + self.set_planning_scene_diff_ = rospy.ServiceProxy("/environment_server/set_planning_scene_diff", SetPlanningSceneDiff) |
3853 | + self.planifier_ = rospy.ServiceProxy('/ompl_planning/plan_kinematic_path', GetMotionPlan) |
3854 | + self.constraint_aware_ik_ = rospy.ServiceProxy("/shadow_right_arm_kinematics/get_constraint_aware_ik", GetConstraintAwarePositionIK) |
3855 | + self.interpolated_ik_params_srv = rospy.ServiceProxy('/r_interpolated_ik_motion_plan_set_params', SetInterpolatedIKMotionPlanParams) |
3856 | + self.interpolated_ik_srv = rospy.ServiceProxy('/r_interpolated_ik_motion_plan', GetMotionPlan) |
3857 | + self.trajectory_filter_ = rospy.ServiceProxy("/trajectory_filter_server/filter_trajectory_with_constraints", FilterJointTrajectoryWithConstraints) |
3858 | + self.get_joint_state_ = rospy.ServiceProxy("/getJointState", getJointState) # TODO use another getJointstateProvided by env Server |
3859 | + |
3860 | + #self.ik_utils = ik_utilities.IKUtilities('right',None,0) # do not wait for service this is not needed for us |
3861 | + |
3862 | + #self.standard_ik_ = rospy.ServiceProxy("/shadow_right_arm_kinematics/get_ik", GetPositionIK) |
3863 | + |
3864 | + |
3865 | + def plan_arm_motion(self, arm_name, planner, palm_target_pose, attached_object=[], hand_target_posture=[]): |
3866 | + """Plan motion for the palm, eventually virtually setting the hand in the pre-grasp posture to manage grasping in small spaces |
3867 | + """ |
3868 | + goal = PoseStamped() |
3869 | + goal.header.frame_id = "world" |
3870 | + |
3871 | + goal.pose=palm_target_pose.pose |
3872 | + |
3873 | + #goal.pose.position.x = 0.470 |
3874 | + #goal.pose.position.y = -0.166 |
3875 | + #goal.pose.position.z = 1.665 |
3876 | + |
3877 | + #goal.pose.orientation.x = 0.375 |
3878 | + #goal.pose.orientation.y = 0.155 |
3879 | + #goal.pose.orientation.z = 0.844 |
3880 | + #goal.pose.orientation.w = 0.351 |
3881 | + |
3882 | + if(planner=="cartesianspace"): |
3883 | + result=self.plan_motion_cartesian_( arm_name, goal ) |
3884 | + else: |
3885 | + result=self.plan_motion_joint_state_( arm_name, goal , attached_object ) |
3886 | + |
3887 | + return result |
3888 | + |
3889 | + def plan_motion_joint_state_(self, arm_name, goal_pose, attached_object_name=[], link_name = "palm"): |
3890 | + self.reset_planning_scene_() |
3891 | + if(attached_object_name): |
3892 | + print "an object is attached" |
3893 | + self.set_planning_scene_(attached_object_name) |
3894 | + |
3895 | + motion_plan_res= GetMotionPlanResponse() |
3896 | + |
3897 | + #first get the ik for the pose we want to go to |
3898 | + ik_solution = None |
3899 | + try: |
3900 | + req = PositionIKRequest() |
3901 | + req.ik_link_name = link_name |
3902 | + req.pose_stamped = goal_pose |
3903 | + req.ik_seed_state.joint_state.name = ARM_NAMES |
3904 | + req.ik_seed_state.joint_state.position = ARM_IK_SEED |
3905 | + ik_solution = self.constraint_aware_ik_.call( req, Constraints(), rospy.Duration(5.0) ) |
3906 | + except rospy.ServiceException, e: |
3907 | + rospy.logerr( "Failed to compute IK "+str(e) ) |
3908 | + motion_plan_res.error_code.val = motion_plan_res.error_code.NO_IK_SOLUTION |
3909 | + return motion_plan_res |
3910 | + |
3911 | + if ik_solution.error_code.val != ik_solution.error_code.SUCCESS: |
3912 | + rospy.logerr("couldn't find an ik solution to go to: " + str(goal_pose)) |
3913 | + motion_plan_res.error_code.val = ik_solution.error_code.val |
3914 | + return motion_plan_res |
3915 | + |
3916 | + #motion_plan_res = None |
3917 | + #try to plan the motion to the target joint_state |
3918 | + try: |
3919 | + motion_plan_request = MotionPlanRequest() |
3920 | + |
3921 | + motion_plan_request.group_name = arm_name #"right_arm" |
3922 | + motion_plan_request.num_planning_attempts = 1 |
3923 | + motion_plan_request.planner_id = "" |
3924 | + motion_plan_request.allowed_planning_time = rospy.Duration(5.0) |
3925 | + |
3926 | + motion_plan_request.expected_path_duration = rospy.Duration(5.0) |
3927 | + motion_plan_request.expected_path_dt = rospy.Duration(0.5) |
3928 | + |
3929 | + # set joint_constraints |
3930 | + for target, name in zip(ik_solution.solution.joint_state.position, ik_solution.solution.joint_state.name): |
3931 | + joint_constraint = JointConstraint() |
3932 | + joint_constraint.joint_name = name |
3933 | + joint_constraint.position = target |
3934 | + joint_constraint.tolerance_below = 0.1 |
3935 | + joint_constraint.tolerance_above = 0.1 |
3936 | + |
3937 | + motion_plan_request.goal_constraints.joint_constraints.append(joint_constraint) |
3938 | + |
3939 | + # start the planner |
3940 | + motion_plan_res = self.planifier_( motion_plan_request ) |
3941 | + |
3942 | + if motion_plan_res.error_code.val != motion_plan_res.error_code.SUCCESS: |
3943 | + rospy.logerr("The planning failed: " + str(motion_plan_res.error_code.val)) |
3944 | + else: |
3945 | + # compute velocity and appropriate times |
3946 | + (times, vels) = self.trajectory_times_and_vels(motion_plan_res.trajectory, [.05]*6, [.1]*6) |
3947 | + #print times |
3948 | + #print vels |
3949 | + for i in range(len(motion_plan_res.trajectory.joint_trajectory.points)): |
3950 | + motion_plan_res.trajectory.joint_trajectory.points[i].velocities = vels[i] |
3951 | + motion_plan_res.trajectory.joint_trajectory.points[i].time_from_start = rospy.Duration(times[i]) |
3952 | + #print motion_plan_res.trajectory.joint_trajectory |
3953 | + except rospy.ServiceException, e: |
3954 | + rospy.logerr( "Failed to plan "+str(e) ) |
3955 | + motion_plan_res.error_code.val = motion_plan_res.error_code.PLANNING_FAILED |
3956 | + |
3957 | + filtered_traj = self.filter_traj_(motion_plan_request, motion_plan_res) |
3958 | + motion_plan_res.trajectory.joint_trajectory = filtered_traj |
3959 | + |
3960 | + return motion_plan_res |
3961 | + |
3962 | + def plan_motion_cartesian_(self, arm_name, goal_pose, link_name="palm"): |
3963 | + self.reset_planning_scene_() |
3964 | + |
3965 | + motion_plan_res = None |
3966 | + try: |
3967 | + motion_plan_request = MotionPlanRequest() |
3968 | + |
3969 | + motion_plan_request.group_name = "right_arm_cartesian" |
3970 | + motion_plan_request.num_planning_attempts = 1 |
3971 | + motion_plan_request.planner_id = "" |
3972 | + motion_plan_request.allowed_planning_time = rospy.Duration(120.0) |
3973 | + |
3974 | + position_constraint = PositionConstraint() |
3975 | + position_constraint.header.stamp = rospy.Time.now() |
3976 | + position_constraint.header.frame_id = goal_pose.header.frame_id |
3977 | + position_constraint.link_name = link_name |
3978 | + position_constraint.position = goal_pose.pose.position |
3979 | + position_constraint.constraint_region_shape.type = Shape.BOX |
3980 | + position_constraint.constraint_region_shape.dimensions.append(0.1) |
3981 | + position_constraint.constraint_region_shape.dimensions.append(0.1) |
3982 | + position_constraint.constraint_region_shape.dimensions.append(0.1) |
3983 | + position_constraint.weight = 1.0 |
3984 | + #position_constraint.constraint_region_orientation.x = 0.307 |
3985 | + #position_constraint.constraint_region_orientation.y = 0.127 |
3986 | + #position_constraint.constraint_region_orientation.z = 0.871 |
3987 | + #position_constraint.constraint_region_orientation.w = 0.362 |
3988 | + position_constraint.constraint_region_orientation.w = 1.0 |
3989 | + motion_plan_request.goal_constraints.position_constraints.append(position_constraint) |
3990 | + |
3991 | + orientation_constraint = OrientationConstraint() |
3992 | + orientation_constraint.header.stamp = rospy.Time.now() |
3993 | + orientation_constraint.header.frame_id = goal_pose.header.frame_id |
3994 | + orientation_constraint.link_name = link_name |
3995 | + |
3996 | + orientation_constraint.orientation.x = 0.351 |
3997 | + orientation_constraint.orientation.y = 0.155 |
3998 | + orientation_constraint.orientation.z = 0.844 |
3999 | + orientation_constraint.orientation.w = 0.375 |
4000 | + |
4001 | + #orientation_constraint.orientation.x = 0.454 |
4002 | + #orientation_constraint.orientation.y = 0.665 |
4003 | + #orientation_constraint.orientation.z = 0.499 |
4004 | + #orientation_constraint.orientation.w = 0.320 |
4005 | + |
4006 | + orientation_constraint.absolute_roll_tolerance = 1.5 |
4007 | + orientation_constraint.absolute_pitch_tolerance = 1.5 |
4008 | + orientation_constraint.absolute_yaw_tolerance = 1.5 |
4009 | + orientation_constraint.weight = 0.5 |
4010 | + motion_plan_request.goal_constraints.orientation_constraints.append(orientation_constraint) |
4011 | + |
4012 | + motion_plan_res = self.planifier_( motion_plan_request ) |
4013 | + |
4014 | + if motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS: |
4015 | + self.display_traj_( motion_plan_res ) |
4016 | + else: |
4017 | + rospy.logerr("The planning failed: " + str(motion_plan_res.error_code.val)) |
4018 | + |
4019 | + except rospy.ServiceException, e: |
4020 | + rospy.logerr( "Failed to plan "+str(e) ) |
4021 | + return False |
4022 | + |
4023 | + def filter_traj_(self, motion_plan_req, motion_plan_res): |
4024 | + try: |
4025 | + req = FilterJointTrajectoryWithConstraintsRequest() |
4026 | + req.group_name="right_arm" |
4027 | + for name in ARM_NAMES: |
4028 | + limit = JointLimits() |
4029 | + limit.joint_name = name |
4030 | + limit.min_position = -1.5 |
4031 | + limit.max_position = 1.5 |
4032 | + limit.has_velocity_limits = True |
4033 | + limit.max_velocity = 0.05 |
4034 | + limit.has_acceleration_limits = True |
4035 | + limit.max_acceleration = 0.1 |
4036 | + req.limits.append(limit) |
4037 | + |
4038 | + req.trajectory = motion_plan_res.trajectory.joint_trajectory |
4039 | + req.goal_constraints=motion_plan_req.goal_constraints |
4040 | + req.path_constraints=motion_plan_req.path_constraints |
4041 | + req.allowed_time = rospy.Duration.from_sec( 5.0 ) |
4042 | + res = self.get_joint_state_.call() |
4043 | + req.start_state.joint_state = res.joint_state |
4044 | + |
4045 | + res = self.trajectory_filter_.call( req ) |
4046 | + except rospy.ServiceException, e: |
4047 | + rospy.logerr("Failed to filter "+str(e)) |
4048 | + return motion_plan_res.trajectory |
4049 | + |
4050 | + return res.trajectory |
4051 | + |
4052 | + def get_interpolated_ik_motion_plan(self, start_pose, target_pose, collision_check=False, |
4053 | + steps_before_abort=1, num_steps=0, |
4054 | + frame='shadowarm_base', max_joint_vels=[0.1]*6, max_joint_accs=[0.3]*6): |
4055 | + |
4056 | + ik_motion_plan_res = self.interpolated_ik_params_srv(num_steps, |
4057 | + math.pi/7.0, |
4058 | + 1, |
4059 | + steps_before_abort, |
4060 | + 0.02, |
4061 | + 0.1, |
4062 | + collision_check, |
4063 | + 1, #start from end |
4064 | + max_joint_vels, |
4065 | + max_joint_accs) |
4066 | + |
4067 | + |
4068 | + self.reset_planning_scene_() |
4069 | + ik_motion_plan_req = GetMotionPlanRequest() |
4070 | + ik_motion_plan_req.motion_plan_request.start_state.joint_state.name = ARM_NAMES |
4071 | + |
4072 | + #joint_state_res = self.get_joint_state_.call() |
4073 | + #start_angles = res.joint_state.positions # one cannot use this directly it contains not only arm but also fingers |
4074 | + ik_motion_plan_req.motion_plan_request.start_state.joint_state.position = [0.3]*6 #start_angles |
4075 | + ik_motion_plan_req.motion_plan_request.start_state.multi_dof_joint_state.poses = [start_pose.pose] |
4076 | + ik_motion_plan_req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = ["palm"] |
4077 | + ik_motion_plan_req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids = [start_pose.header.frame_id] |
4078 | + |
4079 | + pos_constraint = PositionConstraint() |
4080 | + pos_constraint.position = target_pose.pose.position |
4081 | + pos_constraint.header.frame_id = target_pose.header.frame_id |
4082 | + ik_motion_plan_req.motion_plan_request.goal_constraints.position_constraints = [pos_constraint] |
4083 | + |
4084 | + orient_constraint = OrientationConstraint() |
4085 | + orient_constraint.orientation = target_pose.pose.orientation |
4086 | + orient_constraint.header.frame_id = target_pose.header.frame_id |
4087 | + ik_motion_plan_req.motion_plan_request.goal_constraints.orientation_constraints = [orient_constraint] |
4088 | + |
4089 | + #if ordered_collision_operations is not None: |
4090 | + # ik_motion_plan_req.motion_plan_request.ordered_collision_operations = ordered_collision_operations |
4091 | + #rospy.logerr("ik req") |
4092 | + #print ik_motion_plan_req |
4093 | + ik_motion_plan_res = self.interpolated_ik_srv(ik_motion_plan_req) |
4094 | + return ik_motion_plan_res |
4095 | + |
4096 | + def trajectory_times_and_vels(self, traj, max_joint_vels = [.2]*6, max_joint_accs = [.5]*6): |
4097 | + |
4098 | + #min time for each segment |
4099 | + min_segment_time = .01 |
4100 | + |
4101 | + if not traj: |
4102 | + rospy.logdebug("traj path was empty!") |
4103 | + return([], []) |
4104 | + traj_length = len(traj.joint_trajectory.points) |
4105 | + num_joints = len(traj.joint_trajectory.joint_names) |
4106 | + |
4107 | + #sanity-check max vels and accelerations |
4108 | + if not max_joint_vels: |
4109 | + max_joint_vels = [.2]*7 |
4110 | + elif len(max_joint_vels) != num_joints: |
4111 | + rospy.logerr("invalid max_joint_vels!") |
4112 | + return ([], []) |
4113 | + if not max_joint_accs: |
4114 | + max_joint_accs = [.5]*7 |
4115 | + elif len(max_joint_accs) != num_joints: |
4116 | + rospy.logerr("invalid max_joint_accs!") |
4117 | + return ([], []) |
4118 | + for ind in range(num_joints): |
4119 | + if max_joint_vels[ind] <= 0.: |
4120 | + max_joint_vels[ind] = .2 |
4121 | + if max_joint_accs[ind] <= 0.: |
4122 | + max_joint_accs[ind] = .5 |
4123 | + |
4124 | + vels = [[None]*num_joints for i in range(traj_length)] |
4125 | + |
4126 | + #give the trajectory a bit of time to start |
4127 | + segment_times = [None]*traj_length |
4128 | + segment_times[0] = 0.05 |
4129 | + |
4130 | + #find vaguely appropriate segment times, assuming that we're traveling at max_joint_vels at the fastest joint |
4131 | + for ind in range(traj_length-1): |
4132 | + joint_diffs = [math.fabs(traj.joint_trajectory.points[ind+1].positions[x]-traj.joint_trajectory.points[ind].positions[x]) for x in range(num_joints)] |
4133 | + joint_times = [diff/vel for (diff, vel) in zip(joint_diffs, max_joint_vels)] |
4134 | + segment_times[ind+1] = max(joint_times+[min_segment_time]) |
4135 | + |
4136 | + #set the initial and final velocities to 0 for all joints |
4137 | + vels[0] = [0.]*num_joints |
4138 | + vels[traj_length-1] = [0.]*num_joints |
4139 | + |
4140 | + #also set the velocity where any joint changes direction to be 0 for that joint |
4141 | + #and otherwise use the average velocity (assuming piecewise-linear velocities for the segments before and after) |
4142 | + for ind in range(1, traj_length-1): |
4143 | + for joint in range(num_joints): |
4144 | + diff0 = traj.joint_trajectory.points[ind].positions[joint]-traj.joint_trajectory.points[ind-1].positions[joint] |
4145 | + diff1 = traj.joint_trajectory.points[ind+1].positions[joint]-traj.joint_trajectory.points[ind].positions[joint] |
4146 | + if (diff0>0 and diff1<0) or (diff0<0 and diff1>0): |
4147 | + vels[ind][joint] = 0. |
4148 | + else: |
4149 | + vel0 = diff0/segment_times[ind] |
4150 | + vel1 = diff1/segment_times[ind+1] |
4151 | + vels[ind][joint] = (vel0+vel1)/2. |
4152 | + |
4153 | + #increase the times if the desired velocities would require overly large accelerations |
4154 | + for ind in range(1, traj_length): |
4155 | + for joint in range(num_joints): |
4156 | + veldiff = math.fabs(vels[ind][joint]-vels[ind-1][joint]) |
4157 | + acc = veldiff/segment_times[ind] |
4158 | + try: |
4159 | + if acc > max_joint_accs[joint]: |
4160 | + segment_times[ind] = veldiff/max_joint_accs[joint] |
4161 | + except: |
4162 | + pdb.set_trace() |
4163 | + |
4164 | + #turn the segment_times into waypoint times (cumulative) |
4165 | + times = [None]*traj_length |
4166 | + times[0] = segment_times[0] |
4167 | + for ind in range(1, traj_length): |
4168 | + try: |
4169 | + times[ind] = times[ind-1]+segment_times[ind] |
4170 | + except: |
4171 | + rospy.logerr("could not add segment time") |
4172 | + continue |
4173 | + |
4174 | + #return the times and velocities |
4175 | + return (times, vels) |
4176 | + |
4177 | + def reset_planning_scene_(self): |
4178 | + try: |
4179 | + self.set_planning_scene_diff_.call() |
4180 | + except: |
4181 | + rospy.logerr("failed to reset the planning scene") |
4182 | + |
4183 | + def set_planning_scene_(self,attached_object_name): |
4184 | + att_object = AttachedCollisionObject() |
4185 | + att_object.link_name = "palm" |
4186 | + att_object.object.id = attached_object_name |
4187 | + att_object.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT |
4188 | + att_object.object.header.frame_id = "palm" |
4189 | + att_object.object.header.stamp = rospy.Time.now() |
4190 | + object = Shape() |
4191 | + object.type = Shape.CYLINDER |
4192 | + object.dimensions.append(.03) |
4193 | + object.dimensions.append(0.1) |
4194 | + pose = Pose() |
4195 | + pose.position.x = 0.0 |
4196 | + pose.position.y = -0.06 |
4197 | + pose.position.z = 0.06 |
4198 | + pose.orientation.x = 0 |
4199 | + pose.orientation.y = 0 |
4200 | + pose.orientation.z = 0 |
4201 | + pose.orientation.w = 1 |
4202 | + att_object.object.shapes.append(object) |
4203 | + att_object.object.poses.append(pose); |
4204 | + att_object.touch_links= ["ffdistal","mfdistal","rfdistal","lfdistal","thdistal","ffmiddle","mfmiddle","rfmiddle","lfmiddle","thmiddle","ffproximal","mfproximal","rfproximal","lfproximal","thproximal","palm","lfmetacarpal","thbase"] |
4205 | + #att_object.touch_links.push_back("_end_effector"); |
4206 | + |
4207 | + planning_scene_req = SetPlanningSceneDiffRequest() |
4208 | + planning_scene_req.planning_scene_diff.attached_collision_objects.append(att_object); |
4209 | + try: |
4210 | + planning_scene_res=self.set_planning_scene_diff_.call(planning_scene_req) |
4211 | + except: |
4212 | + rospy.logerr("failed to set the planning scene") |
4213 | + |
4214 | +if __name__ =="__main__": |
4215 | + rospy.init_node("planification") |
4216 | + plan = Planification() |
4217 | + |
4218 | + object_pose = PoseStamped() |
4219 | + object_pose.pose.position.x = 0.470 |
4220 | + object_pose.pose.position.y = 0.166 |
4221 | + object_pose.pose.position.z = 1.665 |
4222 | + |
4223 | + object_pose.pose.orientation.x = 0.375 |
4224 | + object_pose.pose.orientation.y = 0.155 |
4225 | + object_pose.pose.orientation.z = 0.844 |
4226 | + object_pose.pose.orientation.w = 0.351 |
4227 | + |
4228 | + |
4229 | + result = plan.plan_arm_motion( "right_arm", "jointspace", object_pose ) |
4230 | + rospy.logerr("got result " + str(result)) |
4231 | + rospy.spin() |
4232 | + |
I fixed some indentation