Problem porting pick_place_tutorial.cpp to python failing with 'No motion plan found. No execution attempted.' error
I am attempting to re-implement the functionality of the pick_place_tutorial.cpp to python. I have done this through a straight line python script which I have included below. I believe the code constructs a planning scene and grasp spec that is identical to the cpp scene and grasp spec, however, when I attempt to execute the pick action at the end of the script it fails with the following from the pick_and_place.py script:
it_tutorials/doc/pick_place/src$ ./pick_and_place.py
[ INFO] [1571700797.286665408]: Loading robot model 'panda'...
[ INFO] [1571700798.393205560]: Ready to take commands for planning group panda_arm.
[ WARN] [1571700800.845780931]: Fail: ABORTED: No motion plan found. No execution attempted.
at the same time the roslaunch panda_moveit_config demo.launch output shows the following errors:
...
[ INFO] [1571700787.558729585]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1571700800.616924369]: Planning attempt 1 of at most 1
[ INFO] [1571700800.641897071]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1571700800.743821028]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 1
[ WARN] [1571700800.744042194]: All supplied grasps failed. Retrying last grasp in verbose mode.
[ INFO] [1571700800.744200144]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1571700800.794486810]: IK failed
[ INFO] [1571700800.844735616]: IK failed
[ INFO] [1571700800.844794259]: Sampler failed to produce a state
[ INFO] [1571700800.844819592]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1571700800.845252413]: Pickup planning completed after 0.126682 seconds
The python script is provided below:
#!/usr/bin/env python
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs
import tf
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
import trajectory_msgs
def wait_for_state_update(scene, box_name, box_is_known=False, box_is_attached=False, timeout=4):
## BEGIN_SUB_TUTORIAL wait_for_scene_update
##
## Ensuring Collision Updates Are Received
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## If the Python node dies before publishing a collision object update message, the message
## could get lost and the box will not appear. To ensure that the updates are
## made, we wait until we see the changes reflected in the
## ``get_attached_objects()`` and ``get_known_object_names()`` lists.
## For the purpose of this tutorial, we call this function after adding,
## removing, attaching or detaching an object in the planning scene. We then wait
## until the updates have been made or ``timeout`` seconds have passed
start = rospy.get_time()
seconds = rospy.get_time()
while (seconds - start < timeout) and not rospy.is_shutdown():
# Test if the box is in attached objects
attached_objects = scene.get_attached_objects([box_name])
is_attached = len(attached_objects.keys()) > 0
# Test if the box is in the scene.
# Note that attaching the box will remove it from known_objects
is_known = box_name in scene.get_known_object_names()
# Test if we are in the expected state
if (box_is_attached == is_attached) and (box_is_known == is_known):
return True
# Sleep so that we give other threads time on the processor
rospy.sleep(0.1)
seconds = rospy.get_time()
# If we exited the while loop without returning then we timed out
return False
## END_SUB_TUTORIAL
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('pick_and_place', anonymous=True)
# Get ...
Were you able to get this working?