def pickup_object(
self,
graspable,
graspable_name,
table_name,
which_arm,
execute=True,
lift_desired_distance=0.1,
lift_min_distance=0.05,
allow_support_collision=True,
ignore_collisions=False,
desired_grasps=None,
wait=rospy.Duration(),
):
"""Picks up a previously detected object.
Parameters:
graspable: an object_manipulation_msgs/GraspableObject msg instance.
This usually comes from a Detector.call_collision_map_processing call.
graspable_name: the name of the object to graps. It is provided by
Detector.call_collision_map_processing.
table_name: the name of the table. Again provided by Detector.call_collision_map_processing.
which_arm: left_arm or right_arm
exectute: True|False, whether to execute the grasp or just check for
feasibility
lift_desired_distance: the desired lift distance
lift_min_distance: the min distance that must be considered
feasible before the grasp is even attempted
allow_support_collision: whether collisions between the gripper and the support
surface should be acceptable during move from pre-grasp to grasp and
during lift. Collisions when moving to the pre-grasp location are still not allowed even
if this is set to true.
ignore_collisions: set this to true if you want to ignore all collisions throughout
the pickup and also move directly to the pre-grasp using
Cartesian controllers
desired_grasps: a list of custom grasps. If not set, pickup will automatically generate grasps.
wait: how long to wait for the action to be successful
Return:
a object_manipulation_msgs.PickupResult msg
"""
rospy.loginfo("Calling the pickup action")
pickup_goal = PickupGoal()
pickup_goal.target = graspable
pickup_goal.collision_object_name = graspable_name
pickup_goal.collision_support_surface_name = table_name
pickup_goal.only_perform_feasibility_test = not execute
pickup_goal.arm_name = which_arm
pickup_goal.lift.direction.header.frame_id = "base_link"
pickup_goal.lift.direction.vector.x = 0
pickup_goal.lift.direction.vector.y = 0
pickup_goal.lift.direction.vector.z = 1
pickup_goal.lift.desired_distance = lift_desired_distance
pickup_goal.lift.min_distance = lift_min_distance
# do not use tactile-based grasping or tactile-based lift
pickup_goal.use_reactive_lift = False
pickup_goal.use_reactive_execution = False
pickup_goal.allow_gripper_support_collision = allow_support_collision
pickup_goal.ignore_collisions = ignore_collisions
if desired_grasps:
pickup_goal.desired_grasps = desired_grasps
self.pickup_client.send_goal_and_wait(pickup_goal, wait)
res = self.pickup_client.get_result()
assert isinstance(res, PickupResult)
manipulation_result = res.manipulation_result
if manipulation_result.value != manipulation_result.SUCCESS:
rospy.logerr("Error during pickup")
return None
self.last_pickup_result = res
return res
def pickup(self, graspable_object, graspable_object_name, object_name):
"""
Try to pick up the given object. Sends a message (PickupGoal from
actionlib_msgs) to the manipularior action server on
/object_manipulator/object_manipulator_pickup/goal
"""
info_tmp = "Picking up " + object_name
rospy.loginfo(info_tmp)
pickup_goal = PickupGoal()
pickup_goal.target = graspable_object
pickup_goal.collision_object_name = graspable_object_name
pickup_goal.collision_support_surface_name = self.gui.collision_support_surface_name
#pickup_goal.additional_link_padding
pickup_goal.ignore_collisions = True
pickup_goal.arm_name = "right_arm"
#pickup_goal.desired_approach_distance = 0.08 This does not exist anymore in the message
#pickup_goal.min_approach_distance = 0.02 This does not exist anymore in the message
direction = Vector3Stamped()
direction.header.stamp = rospy.get_rostime()
direction.header.frame_id = "/base_link"
direction.vector.x = 0
direction.vector.y = 0
direction.vector.z = 1
pickup_goal.lift.direction = direction
#request a vertical lift of 15cm after grasping the object
pickup_goal.lift.desired_distance = 0.1
pickup_goal.lift.min_distance = 0.07
#do not use tactile-based grasping or tactile-based lift
pickup_goal.use_reactive_lift = True
pickup_goal.use_reactive_execution = True
self.pickup_result = self.pickupservice.pick(pickup_goal)
#pickup_client = actionlib.SimpleActionClient('/object_manipulator/object_manipulator_pickup', PickupAction)
#pickup_client.wait_for_server()
#rospy.loginfo("Pickup server ready")
#pickup_client.send_goal(pickup_goal)
#TODO: change this when using the robot
#pickup_client.wait_for_result(timeout=rospy.Duration.from_sec(3600.0))
loginfo("Got Pickup results")
#self.pickup_result = pickup_client.get_result()
#print "Pickup result: "+str(self.pickup_result)
'''
if pickup_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr("The pickup action has failed: " + str(self.pickup_result.manipulation_result.value) )
QMessageBox.warning(self, "Warning",
"Pickup action failed: "+str(self.pickup_result.manipulation_result.value))
for tested_grasp,tested_grasp_result in zip(self.pickup_result.attempted_grasps,self.pickup_result.attempted_grasp_results):
if tested_grasp_result.result_code==7:
self.grasp_display_publisher.publish(tested_grasp)
return -1
'''
if self.pickup_result.manipulation_result.value == ManipulationResult.SUCCESS:
loginfo("Pick succeeded, now lifting")
self.pickupservice.lift(0.07)
else:
loginfo("Pick failed")
return 1
return 0
