moveit pick failed with controller running
I'm using ubuntu 16.04 on a Huawei Matebook-x, with kernel 4.15.0-91-generic. ROS distro: kinetic and version 1.12.14. Gazebo 7. My repository is at https://github.com/yiyinglai/FetchIt.
I was doing something similar to this tutorial and launched gazebo, rviz and controllers using
roslaunch niryo_one_d415_moveit_config niryo_one_d415_coca_cola.launch
and a niryo_one_pick_place node (I removed the place part from the tutorial) using
roslaunch niryo_one_pick_place pick_place.launch
When the object is not obstructing the robot (or when gazebo is not even launched, I believe), the pick operation carries out as expected. As shown in the video below, the robot arm approaches, picks up and retreats with the object attached at the tool_link.
The main logging:
[ INFO] [1586817787.310136812, 17.032000000]: Planning attempt 1 of at most 1
[ INFO] [1586817787.350711758, 17.038000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1586817787.420852165, 17.051000000]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1586817787.422522438, 17.051000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1586817787.469576948, 17.061000000]: RRTConnect: Created 5 states (2 start + 3 goal)
[ INFO] [1586817787.476083876, 17.066000000]: Solution found in 0.054375 seconds
[ INFO] [1586817787.550710569, 17.078000000]: SimpleSetup: Path simplification took 0.074395 seconds and changed from 4 to 2 states
[ INFO] [1586817787.561913385, 17.081000000]: Found successful manipulation plan!
[ INFO] [1586817787.562412652, 17.081000000]: Pickup planning completed after 0.252041 seconds
[ INFO] [1586817798.348857044, 21.571000000]: Completed trajectory execution with status SUCCEEDED ...
When the object is actually in place in gazebo, the pick operation failed as shown in the video below.
Here is the main logging:
[ INFO] [1586814556.588570711, 20.136000000]: Planning attempt 1 of at most 1
[ INFO] [1586814556.622188757, 20.140000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1586814556.723219387, 20.167000000]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0
[ WARN] [1586814556.724581191, 20.168000000]: All supplied grasps failed. Retrying last grasp in verbose mode.
[ INFO] [1586814556.724829331, 20.168000000]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1586814556.743553190, 20.168000000]: IK failed
[ INFO] [1586814556.811681475, 20.182000000]: IK failed
[ INFO] [1586814556.816149899, 20.183000000]: Orientation constraint satisfied for link 'tool_link'. Quaternion desired: 0.500000 0.500000 0.500000 0.500000, quaternion actual: 0.497276 0.500788 0.503431 0.498483, error: x=0.003857, y=0.008457, z=0.001453, tolerance: x=0.010000, y=0.010000, z=0.010000
[ INFO] [1586814556.816250318, 20.183000000]: Position constraint satisfied on link 'tool_link'. Desired: 0.365000, 0.000000, 0.350000, current: 0.364621, 0.000658, 0.349397
[ INFO] [1586814556.816288274, 20.183000000]: Differences 0.000379473 -0.00065784 0.000603184
[ INFO] [1586814556.868221647, 20.192000000]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1586814556.878696365, 20.194000000]: RRTConnect: Starting planning with 1 states ...