Robotics StackExchange | Archived questions

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 pickplacetutorial.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 pickandplace.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 pandamoveitconfig 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 handles to robot and planning scene
robot = moveit_commander.RobotCommander()
group = robot.get_group('panda_arm')
group.set_planning_time(seconds=45.0)

scene = moveit_commander.PlanningSceneInterface()

# Add table 1
box_pose=geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id='panda_link0'
box_pose.pose.orientation.w = 1.0
box_pose.pose.position.x = 0.5
box_pose.pose.position.y = 0
box_pose.pose.position.z = 0.2
rospy.sleep(rospy.Duration(2.0))
scene.add_box(name='table1', size=(0.2,0.4,0.4), pose=box_pose)
wait_for_state_update(scene,'table1',box_is_known=True)

# Add table 2
box_pose.pose.position.x = 0
box_pose.pose.position.y = 0.5
box_pose.pose.position.z = 0.2
scene.add_box(name='table2', size=(0.4,0.2,0.4), pose=box_pose)
wait_for_state_update(scene,'table2',box_is_known=True)

# Add object
box_pose.pose.position.x = 0.5
box_pose.pose.position.y = 0
box_pose.pose.position.z = 0.5
scene.add_box(name='object', size=(0.02,0.02,0.2), pose=box_pose)
wait_for_state_update(scene,'object',box_is_known=True)

# Setup grasp plan
group = robot.get_group('panda_arm')
group.set_planning_time(seconds=45.0)
grasp = moveit_msgs.msg.Grasp()

quat_tf = tf.transformations.quaternion_from_euler(-pi/2, -pi/4, -pi/2)
grasp.grasp_pose.pose.orientation = geometry_msgs.msg.Quaternion(quat_tf[0], quat_tf[1], quat_tf[2], quat_tf[3])

# specify grasp pose
grasp.grasp_pose.pose.position.x = 0.415
grasp.grasp_pose.pose.position.y = 0.
grasp.grasp_pose.pose.position.z = 0.5

# specify pre grasp approach constraints
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

# specify post grasp retreat constraints
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

# open gripper posture
grasp.pre_grasp_posture.joint_names.append('panda_finger_joint1')
grasp.pre_grasp_posture.joint_names.append('panda_finger_joint2')
pt = trajectory_msgs.msg.JointTrajectoryPoint()
pt.positions.append(0.04)
pt.positions.append(0.04)
pt.time_from_start = rospy.Duration(0.5)
grasp.pre_grasp_posture.points.append(pt)

# close grasp posture
grasp.grasp_posture.joint_names.append('panda_finger_joint1')
grasp.grasp_posture.joint_names.append('panda_finger_joint2')
pt = trajectory_msgs.msg.JointTrajectoryPoint()
pt.positions.append(0.0)
pt.positions.append(0.0)
pt.time_from_start = rospy.Duration(0.5)
grasp.grasp_posture.points.append(pt)

group.set_support_surface_name('table1')
group.pick(grasp=grasp,object_name='object')

Note that in the script I have only implemented the pick functionality.

In terms of versions I am running

If I do not add the 'object' box to the scene the visualisation of the pick plan shows that it is successful, however, the execution of the plan proceeds up to the point where the gripper closes and then stops with the following error:

[ WARN] [1571701807.371209353]: Fail: PREEMPTED: Preempted

In this case the the roslaunch pandamoveitconfig demo.launch output shows the following errors:

...
[ERROR] [1571701807.100012260]: Attempting to attach object 'object' to link 'panda_link8' but no geometry specified and such an object does not exist in the collision world
[ERROR] [1571701807.100219477]: Execution of path-completion side-effect failed. Preempting.
[ INFO] [1571701807.100493127]: Fake execution of trajectory
[ INFO] [1571701807.107144360]: Stopping execution due to preempt request
[ INFO] [1571701807.200759234]: Fake trajectory execution cancelled
[ INFO] [1571701807.200815771]: Stopped trajectory execution.
[ INFO] [1571701807.200852742]: Completed trajectory execution with status PREEMPTED ...

I have also tried various permutations of adding attach_box calls however I have been unable to successfully complete the pick motion. My suspicion is that this because the pick action is still including the 'object' in it's collision objects.

I am happy to provide any further information if it helps to isolate the issue.

Asked by johnbmcd on 2019-10-21 19:06:30 UTC

Comments

Were you able to get this working?

Asked by infantasy on 2020-12-06 18:59:22 UTC

Answers

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
    # know that it can ignore table1 initially during planning the 
    # lift.
    move_group.set_support_surface_name('table1')

    # Finally we can plan and execute the pick operation
    move_group.pick('object',grasp)

def place(move_group):
    """Specifies, plans, and executes a place operation."""
    # Create empty PlaceLocation object to specify the place motion 
    placeLocation = msg.PlaceLocation()

    # similiar to frame_id in pick motion
    placeLocation.place_pose.header.frame_id = "panda_link0"

    # First we specify the pose for the place itself in terms 
    # of both the orientation and position
    q = quaternion_from_euler(0, 0, pi / 2)
    placeLocation.place_pose.pose.orientation = Quaternion(q[0],q[1],q[2],q[3])

    placeLocation.place_pose.pose.position.x = 0
    placeLocation.place_pose.pose.position.y = 0.5
    placeLocation.place_pose.pose.position.z = 0.5

    ## Setting pre-place approach ##
    # Defined with respect to frame_id
    placeLocation.pre_place_approach.direction.header.frame_id = "panda_link0"
    # Direction is set as negative z axis
    placeLocation.pre_place_approach.direction.vector.z = - 1.0  
    placeLocation.pre_place_approach.min_distance = 0.095
    placeLocation.pre_place_approach.desired_distance = 0.115

    ## Setting post-grasp retreat
    # Defined with respect to frame_id
    placeLocation.post_place_retreat.direction.header.frame_id = "panda_link0"
    # Direction is set as negative y axis
    placeLocation.post_place_retreat.direction.vector.y = -1.0      
    placeLocation.post_place_retreat.min_distance = 0.1
    placeLocation.post_place_retreat.desired_distance = 0.25

    ## Setting posture of eef after placing object
    # Similar to the pick case
    openGripper(placeLocation.post_place_posture)

    ## Set support surface as table2 ##
    group.set_support_surface_name("table2")

    ## Call place to place the object using the place locations given. ##
    group.place("object", placeLocation)

def addCollisionObjects(planning_scene_interface):
    """Add two tables and an object to the planning scene."""
    # clean the scene in case the script has already been executed
    planning_scene_interface.remove_world_object("table1")
    planning_scene_interface.remove_world_object("table2")
    planning_scene_interface.remove_world_object("object")

    # publish a demo scene
    p = PoseStamped()
    p.header.frame_id = 'panda_link0'
    p.pose.position.x = 0.5
    p.pose.position.y = 0
    p.pose.position.z = 0.2
    p.pose.orientation.w = 1.0
    planning_scene_interface.add_box("table1", p, (0.2, 0.4, 0.4))

    p.pose.position.x = 0
    p.pose.position.y = 0.5
    p.pose.position.z = 0.2
    planning_scene_interface.add_box("table2", p, (0.4, 0.2, 0.4))

    p.pose.position.x = 0.5
    p.pose.position.y = 0
    p.pose.position.z = 0.5
    planning_scene_interface.add_box("object", p, (0.02, 0.02, 0.2))

if __name__=='__main__':
    # Initialise moveit and ROS node
    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)

    # Get handles to the planning scene and robot 
    scene = PlanningSceneInterface()
    robot = RobotCommander()

    # Select planning group
    group = robot.get_group('panda_arm')
    # Set a liberal planner timeout 
    group.set_planning_time(seconds=45.0)

    # IMPORTANT: you must call sleep after the previous line 
    # to ensure the planning scene object is initialised before
    # using it.
    rospy.sleep(2)

    # Setup scene for the pick and place example
    addCollisionObjects(scene)

    rospy.sleep(1)

    # Plan and execute the pick operation
    pick(group)

    rospy.sleep(1)

    # Plan and execute the place operaation
    place(group)

Asked by johnbmcd on 2020-12-07 12:21:24 UTC

Comments

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

Asked by dubao996 on 2020-12-17 08:56:54 UTC