Problem porting pick_place_tutorial.cpp to python failing with 'No motion plan found. No execution attempted.' error

asked 2019-10-21 19:06:30 -0600

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 ...
(more)
edit retag flag offensive close merge delete