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)
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)
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
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 pick_up(self, whicharm="l", NUM_TRIES=5, near_pose=None):
self.pickup_client.cancel_goal()
for i in range(NUM_TRIES):
rospy.loginfo("Attempt %d to grasp " % i )
# call the tabletop detection
rospy.loginfo("Calling tabletop detector")
detection_call = TabletopDetectionRequest()
# we want recognized database objects returned
# set this to false if you are using the pipeline without the database
detection_call.return_clusters = True
#we want the individual object point clouds returned as well
detection_call.return_models = True
detection_call.num_models = 1
detection_call_response = self.object_detection_srv(detection_call).detection
if not detection_call_response:
rospy.loginfo("Tabletop detection service failed")
success=False; break
if detection_call_response.result != detection_call_response.SUCCESS:
rospy.loginfo("Tabletop detection returned error code %d",
detection_call_response.result)
success=False; break
if len(detection_call_response.clusters)==0\
and len(detection_call_response.models)==0 :
rospy.loginfo("The tabletop detector detected the table, "
"but found no objects")
success=False; break
# call collision map processing
rospy.loginfo("Calling collision map processing")
processing_call = TabletopCollisionMapProcessingRequest()
# pass the result of the tabletop detection
processing_call.detection_result = detection_call_response
print detection_call_response.models
import pdb;pdb.set_trace()
# ask for the existing map and collision models to be reset
processing_call.reset_collision_models = True
processing_call.reset_attached_models = True
#ask for the results to be returned in base link frame
processing_call.desired_frame = "base_link"
processing_call_response = self.collision_processing_srv(processing_call)
if not processing_call_response:
rospy.loginfo("Collision map processing service failed")
success=False; break
#the collision map processor returns instances of graspable objects
if len(processing_call_response.graspable_objects) == 0:
rospy.loginfo("Collision map processing returned no graspable objects")
success=False; break
graspable_objects = processing_call_response.graspable_objects
# sort graspable objects
import pdb;
# sort graspable objects
objects_with_offsets = [None]*len(graspable_objects)
#near_point = np.array(list(near_pose[:3]) + [1.0])
for i, obj in enumerate(graspable_objects):
pdb.set_trace()
points = point_cloud_to_mat(obj.cluster)
#centroid =np.reshape(np.mean(points, axis=1), 4) - near_point
#offset = np.linalg.norm(centroid - near_point)
#objects_with_offsets[i] = (offset,obj)
#ordering = sorted(objects_with_offsets, key=lambda item:float((item[0])))
#graspable_objects = [items[1] for items in ordering]
#print [items[0] for items in ordering]
for i, grasp_object in enumerate(graspable_objects):
# call object pickup
rospy.loginfo("Calling the pickup action");
pickup_goal = PickupGoal()
# pass one of the graspable objects returned
# by the collision map processor
#pickup_goal.target = processing_call_response.graspable_objects[0]
pickup_goal.target = grasp_object
# 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 = \
processing_call_response.collision_object_names[i]
# pass the collision name of the table, also returned by the collision
# map processor
pickup_goal.collision_support_surface_name = \
processing_call_response.collision_support_surface_name
# pick up the object with the left arm
if whicharm=="l":
pickup_goal.arm_name = "left_arm"
else:
pickup_goal.arm_name = "right_arm"
# we will be lifting the object along the "vertical" direction
# which is along the z axis in the base_link frame
direction = Vector3Stamped()
direction.header.stamp = rospy.Time.now()
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 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_goal.allow_gripper_support_collision = True
#pickup_goal.ignore_collisions = True
#send the goal
rospy.loginfo("Waiting for the pickup action...")
self.pickup_client.send_goal_and_wait(pickup_goal,rospy.Duration(15))
pickup_result =self.pickup_client.get_result()
assert isinstance(pickup_result, PickupResult)
success = pickup_result.manipulation_result.value ==\
pickup_result.manipulation_result.SUCCESS
if success:
rospy.loginfo("Success! Grasped Object.")
return success
if not success:
rospy.loginfo("failed to grasp object")
return success
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"
#we will be lifting the object along the "vertical" direction
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"
sys.exit()
rospy.loginfo("Calling Pickup")
max_len = 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
rospy.signal_shutdown("")
sys.exit()
sys.exit()
rospy.loginfo("Calling Pickup")
max_len = 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
