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
- Ubuntu 18.04, kernel version 5.0.0-31-generic, and ROS melodic
- moveit_tutorials built from source from branch origin/melodic-devel
- pandamoveitconfig built from source from branch origin/melodic-devel
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
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
Comments
Were you able to get this working?
Asked by infantasy on 2020-12-06 18:59:22 UTC