Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Motion Plan Fail & All Supplied grasps failed #4

Open
rojas70 opened this issue Jun 16, 2014 · 4 comments
Open

Motion Plan Fail & All Supplied grasps failed #4

rojas70 opened this issue Jun 16, 2014 · 4 comments

Comments

@rojas70
Copy link
Contributor

rojas70 commented Jun 16, 2014

Dave, hydro code compiles great.

When running the rosrun clam_pick_place simple_pick_place In generateRandomBlock->pick->generateBlockGrasps:
When the following command is called group->pick(blockname,groups),

  1. The local screen prints out the following error information: [ WARN] [1402899684.071986520, 3635.049000000]: Fail: ABORTED: No motion plan found. No execution attempted. [ERROR] [1402899684.072988983, 3635.049000000]: Pick failed. Retrying.
  2. And the screen for the move_group node prints out the following information:
[ INFO] [1402898171.510113447, 3018.285000000]: Added plan for pipeline 'pick'. Queue is now of size 65
[ INFO] [1402898171.510522689, 3018.285000000]: Added plan for pipeline 'pick'. Queue is now of size 66
[ INFO] [1402898171.510957395, 3018.285000000]: Added plan for pipeline 'pick'. Queue is now of size 67
[ INFO] [1402898171.511557305, 3018.285000000]: Added plan for pipeline 'pick'. Queue is now of size 68
[ INFO] [1402898171.512170740, 3018.285000000]: Added plan for pipeline 'pick'. Queue is now of size 69
[ INFO] [1402898171.512693260, 3018.285000000]: Added plan for pipeline 'pick'. Queue is now of size 70
[ INFO] [1402898171.513163095, 3018.285000000]: Manipulation plan 18 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.513606490, 3018.285000000]: Added plan for pipeline 'pick'. Queue is now of size 71
[ INFO] [1402898171.516414635, 3018.285000000]: Manipulation plan 48 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.519546795, 3018.286000000]: Manipulation plan 116 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.520351639, 3018.286000000]: Manipulation plan 49 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.521627772, 3018.286000000]: Manipulation plan 15 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.524077460, 3018.286000000]: Manipulation plan 83 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.524809469, 3018.287000000]: Added plan for pipeline 'pick'. Queue is now of size 70
[ INFO] [1402898171.527565993, 3018.287000000]: Manipulation plan 117 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.530133979, 3018.287000000]: Manipulation plan 120 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.532713972, 3018.287000000]: Manipulation plan 121 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.540810024, 3018.287000000]: Added plan for pipeline 'pick'. Queue is now of size 66
[ INFO] [1402898171.543822698, 3018.287000000]: Added plan for pipeline 'pick'. Queue is now of size 67
[ INFO] [1402898171.546112794, 3018.287000000]: Added plan for pipeline 'pick'. Queue is now of size 68
[ INFO] [1402898171.547592964, 3018.288000000]: Added plan for pipeline 'pick'. Queue is now of size 69
[ INFO] [1402898171.548795139, 3018.289000000]: Manipulation plan 123 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.552173192, 3018.289000000]: Manipulation plan 55 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.552810446, 3018.289000000]: Added plan for pipeline 'pick'. Queue is now of size 70
[ INFO] [1402898171.553449946, 3018.289000000]: Added plan for pipeline 'pick'. Queue is now of size 71
[ INFO] [1402898171.556655231, 3018.289000000]: Manipulation plan 46 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.561195162, 3018.290000000]: Added plan for pipeline 'pick'. Queue is now of size 67
[ INFO] [1402898171.562439568, 3018.290000000]: Added plan for pipeline 'pick'. Queue is now of size 68
[ INFO] [1402898171.563526958, 3018.290000000]: Added plan for pipeline 'pick'. Queue is now of size 69
[ INFO] [1402898171.564483672, 3018.290000000]: Added plan for pipeline 'pick'. Queue is now of size 70
[ INFO] [1402898171.565006702, 3018.290000000]: Added plan for pipeline 'pick'. Queue is now of size 71
[ INFO] [1402898171.565608587, 3018.290000000]: Added plan for pipeline 'pick'. Queue is now of size 72
[ INFO] [1402898171.566047081, 3018.290000000]: Added plan for pipeline 'pick'. Queue is now of size 73
[ INFO] [1402898171.566487800, 3018.290000000]: Added plan for pipeline 'pick'. Queue is now of size 74
[ INFO] [1402898171.567050801, 3018.290000000]: Added plan for pipeline 'pick'. Queue is now of size 75
[ INFO] [1402898171.567888750, 3018.290000000]: Added plan for pipeline 'pick'. Queue is now of size 76
[ INFO] [1402898171.568254289, 3018.290000000]: Added plan for pipeline 'pick'. Queue is now of size 77
[ INFO] [1402898171.569207605, 3018.290000000]: Manipulation plan 54 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.569663584, 3018.290000000]: Manipulation plan 89 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.573508113, 3018.291000000]: Manipulation plan 47 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.574068047, 3018.291000000]: Manipulation plan 88 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.575039525, 3018.291000000]: Added plan for pipeline 'pick'. Queue is now of size 78
[ INFO] [1402898171.575438664, 3018.291000000]: Added plan for pipeline 'pick'. Queue is now of size 79
[ INFO] [1402898171.576310556, 3018.291000000]: Added plan for pipeline 'pick'. Queue is now of size 80
[ INFO] [1402898171.577474264, 3018.291000000]: Added plan for pipeline 'pick'. Queue is now of size 81
[ INFO] [1402898171.578127154, 3018.291000000]: Added plan for pipeline 'pick'. Queue is now of size 82
[ INFO] [1402898171.579925866, 3018.291000000]: Added plan for pipeline 'pick'. Queue is now of size 83
[ INFO] [1402898171.581452426, 3018.292000000]: Added plan for pipeline 'pick'. Queue is now of size 84
[ INFO] [1402898171.582132413, 3018.292000000]: Added plan for pipeline 'pick'. Queue is now of size 85
[ INFO] [1402898171.582664549, 3018.292000000]: Added plan for pipeline 'pick'. Queue is now of size 86
[ INFO] [1402898171.583414975, 3018.292000000]: Added plan for pipeline 'pick'. Queue is now of size 87
[ INFO] [1402898171.584749365, 3018.292000000]: Added plan for pipeline 'pick'. Queue is now of size 88
[ INFO] [1402898171.585859877, 3018.293000000]: Added plan for pipeline 'pick'. Queue is now of size 89
[ INFO] [1402898171.586378713, 3018.293000000]: Added plan for pipeline 'pick'. Queue is now of size 90
[ INFO] [1402898171.587358589, 3018.293000000]: Added plan for pipeline 'pick'. Queue is now of size 91
[ INFO] [1402898171.587798662, 3018.293000000]: Added plan for pipeline 'pick'. Queue is now of size 92
[ INFO] [1402898171.588197611, 3018.293000000]: Added plan for pipeline 'pick'. Queue is now of size 93
[ INFO] [1402898171.589032022, 3018.293000000]: Added plan for pipeline 'pick'. Queue is now of size 94
[ INFO] [1402898171.591565846, 3018.293000000]: Manipulation plan 114 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.594250802, 3018.293000000]: Manipulation plan 21 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.596496448, 3018.294000000]: Manipulation plan 20 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.597226167, 3018.295000000]: Manipulation plan 115 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.597950864, 3018.295000000]: Manipulation plan 13 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.598706838, 3018.295000000]: Added plan for pipeline 'pick'. Queue is now of size 89
[ INFO] [1402898171.599232901, 3018.295000000]: Manipulation plan 12 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.599594450, 3018.295000000]: Added plan for pipeline 'pick'. Queue is now of size 90
[ INFO] [1402898171.599905555, 3018.295000000]: Added plan for pipeline 'pick'. Queue is now of size 91
[ INFO] [1402898171.600184553, 3018.295000000]: Added plan for pipeline 'pick'. Queue is now of size 92
[ INFO] [1402898171.600497828, 3018.295000000]: Added plan for pipeline 'pick'. Queue is now of size 93
[ INFO] [1402898171.600746391, 3018.295000000]: Added plan for pipeline 'pick'. Queue is now of size 94
[ INFO] [1402898171.601024376, 3018.295000000]: Added plan for pipeline 'pick'. Queue is now of size 95
[ INFO] [1402898171.602302248, 3018.296000000]: Added plan for pipeline 'pick'. Queue is now of size 95
[ INFO] [1402898171.602722000, 3018.296000000]: Added plan for pipeline 'pick'. Queue is now of size 96
[ INFO] [1402898171.603198510, 3018.296000000]: Added plan for pipeline 'pick'. Queue is now of size 97
[ INFO] [1402898171.603631657, 3018.296000000]: Added plan for pipeline 'pick'. Queue is now of size 98
[ INFO] [1402898171.608499536, 3018.296000000]: Manipulation plan 81 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.610859771, 3018.296000000]: Manipulation plan 91 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.613468625, 3018.296000000]: Manipulation plan 90 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.615397963, 3018.297000000]: Manipulation plan 56 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.617564830, 3018.297000000]: Manipulation plan 125 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.618368717, 3018.297000000]: Manipulation plan 80 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.620249175, 3018.297000000]: Manipulation plan 22 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.622143554, 3018.298000000]: Manipulation plan 23 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.623738790, 3018.298000000]: Manipulation plan 124 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.625541647, 3018.298000000]: Manipulation plan 122 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.626917871, 3018.298000000]: Manipulation plan 57 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.627705582, 3018.298000000]: Manipulation plan 11 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.629395467, 3018.298000000]: Manipulation plan 79 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.629966983, 3018.298000000]: Manipulation plan 44 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.632204319, 3018.299000000]: Manipulation plan 113 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.633206060, 3018.299000000]: Manipulation plan 78 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.634816581, 3018.299000000]: Manipulation plan 10 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.637572984, 3018.300000000]: Manipulation plan 59 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.639150099, 3018.300000000]: Manipulation plan 112 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.640383289, 3018.300000000]: Manipulation plan 127 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.641514749, 3018.300000000]: Manipulation plan 24 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.642634693, 3018.300000000]: Manipulation plan 45 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.644514554, 3018.300000000]: Manipulation plan 126 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.648927146, 3018.301000000]: Manipulation plan 92 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.652186244, 3018.301000000]: Manipulation plan 110 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.654584881, 3018.302000000]: Manipulation plan 25 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.657003433, 3018.302000000]: Manipulation plan 76 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.657883814, 3018.302000000]: Manipulation plan 58 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.660164820, 3018.302000000]: Manipulation plan 77 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.662493704, 3018.302000000]: Manipulation plan 42 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.664987061, 3018.302000000]: Manipulation plan 43 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.669191361, 3018.303000000]: Manipulation plan 8 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.671246835, 3018.303000000]: Manipulation plan 95 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.673771172, 3018.303000000]: Manipulation plan 94 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.674333717, 3018.303000000]: Manipulation plan 93 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.677341255, 3018.304000000]: Manipulation plan 111 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.678824913, 3018.304000000]: Manipulation plan 9 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.682198064, 3018.304000000]: Manipulation plan 108 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.683564035, 3018.304000000]: Manipulation plan 109 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.684721053, 3018.304000000]: Manipulation plan 61 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.687571830, 3018.304000000]: Manipulation plan 27 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.691583714, 3018.304000000]: Manipulation plan 74 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.696803832, 3018.305000000]: Manipulation plan 75 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.700051389, 3018.305000000]: Manipulation plan 128 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.703621561, 3018.305000000]: Manipulation plan 26 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.704992073, 3018.305000000]: Manipulation plan 129 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.709647811, 3018.306000000]: Manipulation plan 41 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.710211865, 3018.306000000]: Manipulation plan 40 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.712134408, 3018.306000000]: Manipulation plan 6 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.714337301, 3018.306000000]: Manipulation plan 7 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.716098709, 3018.306000000]: Manipulation plan 60 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.717709806, 3018.307000000]: Manipulation plan 107 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.718554822, 3018.307000000]: Manipulation plan 106 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.720397509, 3018.307000000]: Manipulation plan 38 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.723329199, 3018.307000000]: Manipulation plan 39 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.727613290, 3018.308000000]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.729344449, 3018.308000000]: Manipulation plan 5 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.731591187, 3018.308000000]: Manipulation plan 72 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.737289023, 3018.309000000]: Manipulation plan 62 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.738527525, 3018.309000000]: Manipulation plan 63 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.739615266, 3018.310000000]: Manipulation plan 73 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.743300785, 3018.310000000]: Manipulation plan 97 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.746677229, 3018.310000000]: Manipulation plan 96 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.749080296, 3018.310000000]: Manipulation plan 131 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.751926824, 3018.311000000]: Manipulation plan 130 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.753059842, 3018.311000000]: Manipulation plan 28 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.758558982, 3018.312000000]: Manipulation plan 2 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.760155170, 3018.312000000]: Manipulation plan 29 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.762684830, 3018.312000000]: Manipulation plan 104 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.770393263, 3018.313000000]: Manipulation plan 36 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.773197910, 3018.313000000]: Manipulation plan 37 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.782785854, 3018.314000000]: Manipulation plan 105 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.783915765, 3018.314000000]: Manipulation plan 3 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.787955994, 3018.315000000]: Manipulation plan 71 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.791777411, 3018.315000000]: Manipulation plan 133 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.794703489, 3018.315000000]: Manipulation plan 31 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.799920880, 3018.316000000]: Manipulation plan 99 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.800925457, 3018.316000000]: Manipulation plan 70 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.804374416, 3018.317000000]: Manipulation plan 30 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.805297202, 3018.317000000]: Manipulation plan 132 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.807261179, 3018.317000000]: Manipulation plan 64 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.808711467, 3018.317000000]: Manipulation plan 65 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.811560820, 3018.318000000]: Manipulation plan 135 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.812316413, 3018.318000000]: Manipulation plan 68 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.813971179, 3018.318000000]: Manipulation plan 103 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.816498879, 3018.318000000]: Manipulation plan 101 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.818491726, 3018.318000000]: Manipulation plan 102 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.824073188, 3018.318000000]: Manipulation plan 134 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.829764309, 3018.319000000]: Manipulation plan 69 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.833138753, 3018.320000000]: Manipulation plan 98 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.837351718, 3018.320000000]: Manipulation plan 100 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.843053593, 3018.320000000]: Manipulation plan 66 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1402898171.844758843, 3018.320000000]: Manipulation plan 35 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.849109310, 3018.321000000]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.850508882, 3018.321000000]: Manipulation plan 67 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.852482396, 3018.321000000]: Manipulation plan 32 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1402898171.853267941, 3018.321000000]: Manipulation plan 33 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1402898171.854225186, 3018.321000000]: Manipulation plan 1 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1402898171.854915401, 3018.322000000]: Manipulation plan 34 failed at stage 'reachable & valid pose filter' on thread 1
[Thread 0x7fffbc98c700 (LWP 15002) exited]
[Thread 0x7fffaeffd700 (LWP 15005) exited]
[ WARN] [1402898171.909609234, 3018.327000000]: All supplied grasps failed. Retrying last grasp in verbose mode.
[Thread 0x7fffaf7fe700 (LWP 15004) exited]
[Thread 0x7fffaffff700 (LWP 15003) exited]
[New Thread 0x7fffaeffd700 (LWP 15006)]
[New Thread 0x7fffaf7fe700 (LWP 15007)]
[New Thread 0x7fffaffff700 (LWP 15008)]
[New Thread 0x7fffbc98c700 (LWP 15009)]
[ INFO] [1402898171.952536275, 3018.335000000]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1402898171.955063028, 3018.339000000]: Found a contact between 'Block1' (type 'Object') and 'l_gripper_pincher_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1402898171.956387976, 3018.339000000]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1402898171.957153642, 3018.339000000]: Manipulation plan 34 failed at stage 'reachable & valid pose filter' on thread 2
[Thread 0x7fffbc98c700 (LWP 15009) exited]
[Thread 0x7fffaf7fe700 (LWP 15007) exited]
[ INFO] [1402898171.980716744, 3018.339000000]: Pickup planning completed after 0.806943 seconds
[Thread 0x7fffaffff700 (LWP 15008) exited]
[Thread 0x7fffaeffd700 (LWP 15006) exited]

Would this be related to the collision between Block1 and the l_gripper_finger_link or could it be related to approximate kinematics as mentioned in: https://groups.google.com/forum/#!searchin/moveit-users/no$20motion$20plan$20found/moveit-users/cup7Z-1bug0/ivp9a2ZNsZcJ ?

There is a very similar problem mentioned in the moveit group (https://groups.google.com/forum/#!searchin/moveit-users/no$20motion$20plan$20found/moveit-users/_M0mf-R7AvI/cV4v755bwEQJ) but I have not yet figured it out.
@dylanvaughn
Copy link
Collaborator

I am also seeing this with the pick and place demo and the latest clam code from indigo-devel. At first I was getting an error in the moveit core libs but fixed that by using the latest moveit code from source in my clam workspace.

I am running the demo like this:

$ roslaunch clam_bringup lowlevel_simulator.launch
$ roslaunch clam_moveit_config move_group.launch verbose:=true debug:=true
$ roslaunch clam_moveit_config moveit_rviz.launch
$ roslaunch clam_pick_place pick_place.launch verbose:=true debug:=true

and am getting this output in the move_group terminal after a failed pick attempt - usually it is this:

[ WARN] [1419880013.907854044]: All supplied grasps failed. Retrying last grasp in verbose mode.
[New Thread 0x7fffb57fa700 (LWP 19212)]
[New Thread 0x7fffb5ffb700 (LWP 19213)]
[New Thread 0x7fffb67fc700 (LWP 19214)]
[New Thread 0x7fffb6ffd700 (LWP 19215)]
[ INFO] [1419880013.914476004]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1419880013.926159928]: IK failed
[ INFO] [1419880013.953612387]: IK failed
[ INFO] [1419880013.956352267]: IK failed
[ INFO] [1419880013.956390577]: Sampler failed to produce a state
[ INFO] [1419880013.956411766]: Manipulation plan 1 failed at stage 'reachable & valid pose filter' on thread 0
[Thread 0x7fffb6ffd700 (LWP 19215) exited]
[Thread 0x7fffb67fc700 (LWP 19214) exited]
[Thread 0x7fffb57fa700 (LWP 19212) exited]
[ INFO] [1419880013.958066601]: Pickup planning completed after 3.366494 seconds

but once it was this:

[ WARN] [1419880054.024804945]: All supplied grasps failed. Retrying last grasp in verbose mode.
[New Thread 0x7fffb57fa700 (LWP 19249)]
[New Thread 0x7fffb5ffb700 (LWP 19250)]
[New Thread 0x7fffb67fc700 (LWP 19251)]
[New Thread 0x7fffb6ffd700 (LWP 19252)]
[ INFO] [1419880054.026400093]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1419880054.036831667]: Found a contact between 'base_link' (type 'Robot link') and 'shoulder_pitch_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1419880054.036895535]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1419880054.037461546]: Orientation constraint satisfied for link 'gripper_roll_link'. Quaternion desired: 0.000000 -0.000000 0.707107 0.707107, quaternion actual: -0.001798 -0.001698 0.709919 0.704279, error: x=0.004945, y=0.000121, z=0.007977, tolerance: x=0.010000, y=0.010000, z=0.010000
[ INFO] [1419880054.037517038]: Position constraint violated on link 'gripper_roll_link'. Desired: 0.350000, -0.100000, 0.020000, current: 0.345304, -0.094064, 0.019757
[ INFO] [1419880054.037551202]: Differences 0.00469566 -0.00593592 0.000243243
[ INFO] [1419880054.079623823]: Found a contact between 'base_link' (type 'Robot link') and 'shoulder_pitch_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1419880054.079695769]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1419880054.080148737]: Orientation constraint satisfied for link 'gripper_roll_link'. Quaternion desired: 0.000000 -0.000000 0.707107 0.707107, quaternion actual: 0.000704 0.002694 0.706237 0.707970, error: x=0.004808, y=0.002808, z=0.002457, tolerance: x=0.010000, y=0.010000, z=0.010000
[ INFO] [1419880054.080185941]: Position constraint violated on link 'gripper_roll_link'. Desired: 0.350000, -0.100000, 0.020000, current: 0.344452, -0.094603, 0.019870
[ INFO] [1419880054.080215181]: Differences 0.00554827 -0.00539707 0.000129861
[ INFO] [1419880054.114410380]: Found a contact between 'base_link' (type 'Robot link') and 'shoulder_pitch_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1419880054.114455911]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1419880054.114813763]: Orientation constraint satisfied for link 'gripper_roll_link'. Quaternion desired: 0.000000 -0.000000 0.707107 0.707107, quaternion actual: -0.003875 -0.000947 0.705291 0.708906, error: x=0.006808, y=0.004159, z=0.005098, tolerance: x=0.010000, y=0.010000, z=0.010000
[ INFO] [1419880054.114848430]: Position constraint violated on link 'gripper_roll_link'. Desired: 0.350000, -0.100000, 0.020000, current: 0.344747, -0.093182, 0.020416
[ INFO] [1419880054.114875477]: Differences 0.00525277 -0.006818 -0.000416043
[ INFO] [1419880054.114898877]: Sampler failed to produce a state
[ INFO] [1419880054.114922702]: Manipulation plan 32 failed at stage 'reachable & valid pose filter' on thread 2
[Thread 0x7fffb5ffb700 (LWP 19250) exited]
[Thread 0x7fffb6ffd700 (LWP 19252) exited]
[Thread 0x7fffb67fc700 (LWP 19251) exited]
[Thread 0x7fffb57fa700 (LWP 19249) exited]
[ INFO] [1419880054.117738265]: Pickup planning completed after 3.536156 seconds

I am also attached a screenshot of the rviz visualization - any ideas on how to debug?

clam_moveit_pick_place

@dylanvaughn
Copy link
Collaborator

OK I have made a bit of progress. I moved the block locations a little in moveit_blocks.h in moveit_simple_grasps package and then it was able to find the block and tried to plan a path but there was an error in the controller code :

From the pick and place terminal:

[ INFO] [1419881069.613040931]: Generated 34 grasps.
[ INFO] [1419881069.615142955]: Generated 68 grasps.
[ INFO] [1419881069.616607543]: Generated 102 grasps.
[ INFO] [1419881069.617736162]: Generated 136 grasps.
[ WARN] [1419881088.442349666]: Fail: ABORTED: Solution found but controller failed during execution
[ERROR] [1419881088.442654949]: Pick failed.
[ INFO] [1419881088.442668718]: Retry? (y/n)

From the move_group terminal:

[ INFO] [1419881085.172649273]: Manipulation plan 60 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1419881085.197547651]: Attaching world object 'Block1' to link 'gripper_roll_link'
[ INFO] [1419881085.197993628]: Attached object 'Block1' to link 'gripper_roll_link'
[ INFO] [1419881085.217595073]: Manipulation plan 5 failed at stage 'reachable & valid pose filter' on thread 0
[New Thread 0x7fff9effd700 (LWP 28662)]
[ INFO] [1419881085.258537444]: Manipulation plan 39 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1419881085.263702269]: LBKPIECE1: Starting planning with 1 states already in datastructure
[Thread 0x7fff9effd700 (LWP 28662) exited]
[ INFO] [1419881085.274721881]: Manipulation plan 38 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1419881085.303703579]: LBKPIECE1: Created 24 (9 start + 15 goal) states in 18 cells (9 start (9 on boundary) + 9 goal (9 on boundary))
[ INFO] [1419881085.303815380]: Solution found in 0.046069 seconds
[ INFO] [1419881085.310102520]: SimpleSetup: Path simplification took 0.006175 seconds and changed from 16 to 2 states
[ INFO] [1419881085.317754469]: Found successful manipulation plan!
[Thread 0x7fffb4ff9700 (LWP 28660) exited]
[ INFO] [1419881085.363930516]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 0
[Thread 0x7fffb57fa700 (LWP 28658) exited]
[ INFO] [1419881085.371541739]: Manipulation plan 97 failed at stage 'reachable & valid pose filter' on thread 1
[Thread 0x7fffb6ffd700 (LWP 28659) exited]
[ INFO] [1419881085.434258902]: Manipulation plan 96 failed at stage 'reachable & valid pose filter' on thread 3
[Thread 0x7fff9f7fe700 (LWP 28661) exited]
[ INFO] [1419881085.437977963]: Pickup planning completed after 2.160031 seconds
[New Thread 0x7fff9f7fe700 (LWP 28663)]
[New Thread 0x7fffb5ffb700 (LWP 28664)]
[ WARN] [1419881088.295890098]: Controller handle clam_trajectory_controller reports status ABORTED
[Thread 0x7fffb5ffb700 (LWP 28664) exited]
[Thread 0x7fff9f7fe700 (LWP 28663) exited]

From the lowlevel_simulator terminal:

[ INFO] [1419881088.291119147]: Trajectory execution successfully completed
[ERROR] [1419881088.294399496]: Unrecognized command -0.925024
[ INFO] [1419881088.295174873]: 
 inside process Trajectory with msg:
header: 
  seq: 0
  stamp: 0.000000000
  frame_id: /base_link
joint_names[]
  joint_names[0]: gripper_roll_joint
points[]
  points[0]: 
    positions[]
      positions[0]: 0.0459599
    velocities[]
      velocities[0]: 0
    accelerations[]
      accelerations[0]: -0.029919
    effort[]
    time_from_start: 0.000000000
  points[1]: 
    positions[]
      positions[0]: 0.0459599
    velocities[]
      velocities[0]: 0
    accelerations[]
      accelerations[0]: -0.029919
    effort[]
    time_from_start: 4.000000000

[ERROR] [1419881088.295332023]: Unable to locate joint elbow_pitch_joint in the commanded trajectory

So getting closer! I will trouble-shoot this later today - any advice welcome :)

@dylanvaughn
Copy link
Collaborator

The latest error "Unable to locate joint elbow_pitch_joint" seems to be because MoveIt is sending a Trajectory message with only the gripper_roll_joint specified and the dynamixel_simulator_interface joint_trajectory_action_controller.cpp file seems to be expecting all the Trajectory messages to have all the joints specified.

@xuxinwang
Copy link

Dear Dylan
I meet the same problem that the error outside of tolerance: x=0.010000, y=0.010000, z=0.010000
So, I want to know do you solve the problem?

Many thanks.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
3 participants
@dylanvaughn @rojas70 @xuxinwang