dylanvaughn
commented
9 years ago 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:=trueand 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 secondsbut 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 secondsI am also attached a screenshot of the rviz visualization - any ideas on how to debug?
xuxinwang
Dave, hydro code compiles great.
When running the
rosrun clam_pick_place simple_pick_placeIn generateRandomBlock->pick->generateBlockGrasps: When the following command is calledgroup->pick(blockname,groups),[ WARN] [1402899684.071986520, 3635.049000000]: Fail: ABORTED: No motion plan found. No execution attempted. [ERROR] [1402899684.072988983, 3635.049000000]: Pick failed. Retrying.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.