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
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
goal.lift.desired_distance = .1
goal.lift.min_distance = .05
# do not use tactile-based grasping or tactile-based lift
goal.use_reactive_execution = 0
goal.use_reactive_lift = 0
# send the goal
try:
grasper_grasp_action_client.send_goal(goal)
except rospy.ServiceException, e:
rospy.logerr("error when calling %s: %e" (grasper_grasp_name, e))
if len(c.points) > max_len:
max_len = len(c.points)
index = i
rospy.loginfo(
"Grasping object %d with %d points" %
(index, len(detection_reply.detection.clusters[index].points)))
pickup_goal = PickupGoal()
pickup_goal.target = processing_reply.graspable_objects[index]
pickup_goal.collision_object_name = processing_reply.collision_object_names[
index]
pickup_goal.collision_support_surface_name = processing_reply.collision_support_surface_name
pickup_goal.arm_name = "left_arm"
pickup_goal.desired_approach_distance = 0.2
pickup_goal.min_approach_distance = 0.05
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
# request a vertical lift of 10cm after grasping the object
pickup_goal.lift.desired_distance = 0.1
pickup_goal.lift.min_distance = 0.05
# do not use tactile-based grasping or tactile-based lift
pickup_goal.use_reactive_lift = False
pickup_goal.use_reactive_execution = False
pickup_client.send_goal_and_wait(pickup_goal, rospy.Duration(120.0))
index = 0
for i,c in enumerate(detection_reply.detection.clusters):
if len(c.points) > max_len:
max_len = len(c.points)
index = i
rospy.loginfo("Grasping object %d with %d points"%(index, len(detection_reply.detection.clusters[index].points)))
pickup_goal = PickupGoal()
pickup_goal.target = processing_reply.graspable_objects[index]
pickup_goal.collision_object_name = processing_reply.collision_object_names[index]
pickup_goal.collision_support_surface_name = processing_reply.collision_support_surface_name
pickup_goal.arm_name = "left_arm"
pickup_goal.desired_approach_distance = 0.2
pickup_goal.min_approach_distance = 0.05
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
# request a vertical lift of 10cm after grasping the object
pickup_goal.lift.desired_distance = 0.1
pickup_goal.lift.min_distance = 0.05
# do not use tactile-based grasping or tactile-based lift
pickup_goal.use_reactive_lift = False
pickup_goal.use_reactive_execution = False
pickup_client.send_goal_and_wait(pickup_goal, rospy.Duration(120.0))
