Ask Your Question
0

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

johnbmcd gravatar image

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

Comments

Were you able to get this working?

infantasy gravatar image infantasy  ( 2020-12-06 17:59:22 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-12-07 11:21:24 -0600

johnbmcd gravatar image

Hi,

Yes I managed to get it working thanks some help from the moveit developers I spotted that I had an error in the way I was specifying one of the frame_id's (github issue here: ).

Working code below:

#!/usr/bin/env python

###################################
# This file provides a complete example of a pick and place task
# using the moveit_commander python api. 
###################################

import sys
import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped, Quaternion
from math import pi
from moveit_msgs import msg
from trajectory_msgs.msg import JointTrajectoryPoint
from tf.transformations import quaternion_from_euler

def closedGripper(posture):
    """Specifies closed grasp posture for the pick and place motions"""
    # specify which end effector joints are involved in the grasp
    posture.joint_names.append('panda_finger_joint1')
    posture.joint_names.append('panda_finger_joint2')
    pt = JointTrajectoryPoint()

    # now specify where they should be positioned in order to 
    # close the hand
    pt.positions.append(0.0)
    pt.positions.append(0.0)
    # ... and how long it should take
    pt.time_from_start = rospy.Duration(0.5)
    posture.points.append(pt)

def openGripper(posture):
    """Specifies open grasp posture for the pick and place motions"""
    # specify which end effector joints are involved in the grasp
    posture.joint_names.append('panda_finger_joint1')
    posture.joint_names.append('panda_finger_joint2')
    pt = JointTrajectoryPoint()

    # now specify where they should be positioned in order to 
    # open the hand
    pt.positions.append(0.04)
    pt.positions.append(0.04)
    # ... and how it should take to open the hand
    pt.time_from_start = rospy.Duration(0.5)
    posture.points.append(pt)

def pick(move_group):
    """Specifies, plans, and executes a pick operation."""
    # Create empty grasp object to parameterise the pick motion
    grasp = msg.Grasp()

    # A grasp has to be specified relative to some frame
    # of reference. Here we specify the base of the robot
    # as the frame for the grasp
    grasp.grasp_pose.header.frame_id = 'panda_link0'

    # First we specify the pose for the grasp itself in terms 
    # of both the orientation and position
    q = quaternion_from_euler(-pi/2, -pi/4, -pi/2)
    grasp.grasp_pose.pose.orientation = Quaternion(q[0],q[1],q[2],q[3])
    grasp.grasp_pose.pose.position.x = 0.415
    grasp.grasp_pose.pose.position.y = 0
    grasp.grasp_pose.pose.position.z = 0.5

    # Next we specify the pre-grasp approach i.e. how should the
    # end-effector approach the object. 
    grasp.pre_grasp_approach.direction.header.frame_id = 'panda_link0'
    grasp.pre_grasp_approach.direction.vector.x = 1.0
    grasp.pre_grasp_approach.min_distance = 0.095
    grasp.pre_grasp_approach.desired_distance = 0.115

    # Then we specify the post-grasp retreat  i.e. how should the
    # end-effector lift the object from the support surface
    grasp.post_grasp_retreat.direction.header.frame_id = 'panda_link0'
    grasp.post_grasp_retreat.direction.vector.z = 1.0
    grasp.post_grasp_retreat.min_distance = 0.1
    grasp.post_grasp_retreat.desired_distance = 0.25

    # Finally we specify the parameters for the open and close
    # posture of the grasp 
    openGripper(grasp.pre_grasp_posture)

    closedGripper(grasp.grasp_posture)

    # When the robot picks up the object it then includes the object
    # as part of its planning i.e. to ensure both the robot and 
    # object do not come into collision with another object. Since
    # the object is in contact with table1 we must let the planner ...
(more)
edit flag offensive delete link more

Comments

hi,i can not find out your solutions to this question

dubao996 gravatar image dubao996  ( 2020-12-17 07:56:54 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

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

Seen: 180 times

Last updated: Dec 07 '20