def execute(self, graspable_object, graspable_object_name,
collision_support_surface_name,
pickup_direction=None, arm_name="right_arm",
desired_approach_distance = 0.05,
min_approach_distance = 0.02,
desired_lift_distance = 0.25,
min_lift_distance = 0.2):
"""
Pickup the object.
"""
pickup_goal = PickupGoal()
pickup_goal.target = graspable_object
pickup_goal.collision_object_name = graspable_object_name
pickup_goal.collision_support_surface_name = collision_support_surface_name
pickup_goal.arm_name = arm_name
pickup_goal.desired_approach_distance = desired_approach_distance
pickup_goal.min_approach_distance = min_approach_distance
if pickup_direction != None:
pickup_goal.lift.direction = pickup_direction
else:
#follow z direction
pickup_direction = Vector3Stamped()
pickup_direction.header.stamp = rospy.get_rostime()
pickup_direction.header.frame_id = "/base_link";
pickup_direction.vector.x = 0;
pickup_direction.vector.y = 0;
pickup_direction.vector.z = 1;
pickup_goal.lift.direction = pickup_direction;
#request a vertical lift of 15cm after grasping the object
pickup_goal.lift.desired_distance = desired_lift_distance
pickup_goal.lift.min_distance = min_lift_distance
#do not use tactile-based grasping or tactile-based lift
pickup_goal.use_reactive_lift = True;
pickup_goal.use_reactive_execution = True;
self.pickup_client.send_goal(pickup_goal)
#timeout after 1sec
#TODO: change this when using the robot
self.pickup_client.wait_for_result(timeout=rospy.Duration.from_sec(90.0))
rospy.loginfo("Got Pickup results")
pickup_result = self.pickup_client.get_result()
if self.pickup_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr("The pickup action has failed: " + str(pickup_result.manipulation_result.value) )
return -1
return pickup_result
def pickup_object(self,
collision_map_processing_msg,
which_arm,
index=0,
desired_approach_distance=0.2,
min_approach_distance=0.05,
lift_desired_distance=0.1,
lift_min_distance=0.05,
wait=rospy.Duration()):
'''
Picks up a previously detected object
@param collision_map_processing_msg: The message resulting from calling
a TabletopCollisionMapProcessing service.
@param index: The index of the object to grasp, if more than one
@param which_arm: left_arm or right_arm
@param desired_approach_distance: how far the pre-grasp should ideally
be away from the grasp
@param min_approach_distance: how much distance between pre-grasp
and grasp must actually be feasible for the grasp not to be rejected
@param lift_desired_distance: the desired lift distance
@param lift_min_distance: the min distance that must be considered
feasible before the grasp is even attempted
@param wait: how long to wait for the action to be successful
'''
pickup_goal = PickupGoal()
pickup_goal.target = collision_map_processing_msg.graspable_objects[
index]
pickup_goal.collision_object_name = collision_map_processing_msg.collision_object_names[
index]
pickup_goal.collision_support_surface_name = collision_map_processing_msg.collision_support_surface_name
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
self.pickup_client.send_goal_and_wait(pickup_goal, wait)
TabletopCollisionMapProcessing)
req = TabletopCollisionMapProcessingRequest()
req.detection_result = tdr
req.reset_collision_models = True
req.reset_attached_models = True
req.reset_static_map = True
req.take_static_collision_map = True
req.desired_frame = 'base_link'
coll_map_res = update_collision_map(req)
rospy.loginfo('found %d clusters, grabbing first' %
len(coll_map_res.graspable_objects))
#obj_pcluster = res.clusters[0]
req = PickupGoal()
req.arm_name = 'left_arm'
req.target = coll_map_res.graspable_objects[0]
#req.target.type = GraspableObject.POINT_CLUSTER
#req.target.cluster = obj_pcluster
req.desired_approach_distance = 0.05
req.min_approach_distance = 0.03
req.lift.direction.header.frame_id = 'base_link'
req.lift.direction.vector.x = 0
req.lift.direction.vector.y = 0
req.lift.direction.vector.z = 1
req.lift.desired_distance = 0.03
req.lift.min_distance = 0.02
req.collision_object_name = coll_map_res.collision_object_names[0]
req.collision_support_surface_name = coll_map_res.collision_support_surface_name
req.use_reactive_execution = False
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
sys.exit()
# print out detected objects
for (ind, object) in enumerate(col_res.graspable_objects):
if object.type == GraspableObject.DATABASE_MODEL:
rospy.loginfo("object %d: recognized object with id %d" %
(ind, object.model_pose.model_id))
else:
rospy.loginfo("object %d: point cluster with %d points" %
(ind, len(object.cluster.points)))
'''
object pickup
'''
rospy.loginfo("grasping object...")
# call object pickup
goal = PickupGoal()
# pass one of the graspable objects returned by the collision map processor
goal.target = col_res.graspable_objects[0]
# pass the name that the object has in the collision environment, this name was also returned by the collision map processor
goal.collision_object_name = col_res.collision_object_names[0]
# pass the collision name of the table, also returned by the collision map processor
goal.collision_support_surface_name = col_res.collision_support_surface_name
# pick up the object with the left arm
goal.arm_name = "left_arm"
# specify the desired distance between pre-grasp and final grasp
goal.desired_approach_distance = .1
goal.min_approach_distance = .05
# we will be lifting the object along the vertical direction which is along the z axis in the base_link frame
goal.lift = GripperTranslation()
goal.lift.direction = create_vector3_stamped([0., 0., 1.], 'base_link')
# request a vertical lift of 10cm after grasping the object
dist = sqrt(pow(x_dist, 2) + pow(y_dist, 2))
rospy.loginfo(dist)
if (dist < min_dist):
min_dist = dist
min_index = index
index += 1
pickup_client = SimpleActionClient(
"/object_manipulator/object_manipulator_pickup", PickupAction)
pickup_client.wait_for_server()
rospy.loginfo("Calling the pickup action")
pickup_goal = PickupGoal()
pickup_goal.target = col_res.graspable_objects[min_index]
#pass the name that the object has in the collision environment
#this name was also returned by the collision map processor
pickup_goal.collision_object_name = col_res.collision_object_names[
min_index]
#pass the collision name of the table, also returned by the collision
#map processor
pickup_goal.collision_support_surface_name = col_res.collision_support_surface_name
#pick up the object with the left arm
pickup_goal.arm_name = "left_arm"
