class ObjectChooser(QWidget): """ Display the list of found objects in a table. Make PickUp calls to manipulator action server on selected objects. """ def __init__(self, parent, gui, title): QWidget.__init__(self) self.gui = gui self.grasp = None self.title = QLabel() self.title.setText(title) self.draw_functions = draw_functions.DrawFunctions('grasp_markers') self.pickup_result = None self.listener = tf.TransformListener() self.grasp_display_publisher = rospy.Publisher("/grasp_display", Grasp) self.pickupservice = Execution() def draw(self): self.frame = QFrame(self) self.tree = QTreeWidget() self.connect(self.tree, SIGNAL('itemDoubleClicked (QTreeWidgetItem *, int)'), self.double_click) self.tree.setHeaderLabels(["Object Name", "Maker", "tags"]) self.tree.resizeColumnToContents(0) self.tree.resizeColumnToContents(1) self.tree.resizeColumnToContents(2) self.layout = QVBoxLayout() self.layout.addWidget(self.title) self.layout.addWidget(self.tree) self.frame.setLayout(self.layout) layout = QVBoxLayout() layout.addWidget(self.frame) self.frame.show() self.setLayout(layout) self.show() def double_click(self, item, value): self.object_name = str(item.text(0)) self.object = self.gui.found_objects[self.object_name] graspable_object = self.object.graspable_object # draw a bounding box around the selected object (box_pose, box_dims) = self.call_find_cluster_bounding_box( graspable_object.cluster) if box_pose == None: return box_mat = pose_to_mat(box_pose.pose) rospy.logerr("box_pose %f %f %f q: %f %f %f %f", box_pose.pose.position.x, box_pose.pose.position.y, box_pose.pose.position.z, box_pose.pose.orientation.x, box_pose.pose.orientation.y, box_pose.pose.orientation.z, box_pose.pose.orientation.w) box_ranges = [[-box_dims.x / 2, -box_dims.y / 2, -box_dims.z / 2], [box_dims.x / 2, box_dims.y / 2, box_dims.z / 2]] self.box_pose = box_pose self.draw_functions.draw_rviz_box(box_mat, box_ranges, '/world', ns='bounding box', color=[0, 0, 1], opaque=0.25, duration=60) # call the pickup service res = self.pickup(graspable_object, self.object.graspable_object_name, self.object_name) if res == 0: #correctly picked up #TODO: set up place_location executed_grasp = self.pickup_result.grasp initial_pose = PoseStamped() initial_pose.header.stamp = rospy.get_rostime() initial_pose.header.frame_id = "/world" initial_pose.pose.position.x = self.box_pose.pose.position.x + 0.1 initial_pose.pose.position.y = self.box_pose.pose.position.y - 0.3 initial_pose.pose.position.z = self.box_pose.pose.position.z - box_dims.z / 2 # graspable object is from bottom but bounding box is at center ! q = transformations.quaternion_about_axis(-0.05, (0, 0, 1)) initial_pose.pose.orientation.x = self.box_pose.pose.orientation.x #q[0] initial_pose.pose.orientation.y = self.box_pose.pose.orientation.y #q[1] initial_pose.pose.orientation.z = self.box_pose.pose.orientation.z #q[2] initial_pose.pose.orientation.w = self.box_pose.pose.orientation.w #q[3] self.list_of_poses = self.compute_list_of_poses( initial_pose, graspable_object, executed_grasp) #print "list of pose",self.list_of_poses self.place_object(graspable_object, self.object.graspable_object_name, self.object_name, self.list_of_poses) def place_click(self): if (self.pickup_result): grasp = self.pickup_result.grasp self.pickupservice.pre_grasp_exec(grasp, 10.0) else: self.pickupservice.grasp_release_exec(10.0) initial_pose = PoseStamped() initial_pose.header.stamp = rospy.get_rostime() initial_pose.header.frame_id = "/world" #"/fixed" #fake hand to world pose initial_pose.pose.position.x = 0.4 #self.box_pose.pose.position.x+0.0 initial_pose.pose.position.y = 0.091 # self.box_pose.pose.position.y-0.2 initial_pose.pose.position.z = 1.25 #self.box_pose.pose.position.z+0.05 q = transformations.quaternion_about_axis(-0.05, (0, 0, 1)) initial_pose.pose.orientation.x = 0.41 #self.box_pose.pose.orientation.x#q[0] initial_pose.pose.orientation.y = 0.695 #self.box_pose.pose.orientation.y#q[1] initial_pose.pose.orientation.z = 0.52 #self.box_pose.pose.orientation.z#q[2] initial_pose.pose.orientation.w = 0.284 #self.box_pose.pose.orientation.w#q[3] executed_grasp = Grasp() executed_grasp.grasp_pose = copy.deepcopy(initial_pose.pose) #fake obj world origin pose initial_pose.pose.position.x = 0.43 initial_pose.pose.position.y = 0.149 initial_pose.pose.position.z = 1.088 initial_pose.pose.orientation.x = 0.0 #self.box_pose.pose.orientation.x#q[0] initial_pose.pose.orientation.y = 0.0 #self.box_pose.pose.orientation.y#q[1] initial_pose.pose.orientation.z = 0.567 #self.box_pose.pose.orientation.z#q[2] initial_pose.pose.orientation.w = 0.824 #self.box_pose.pose.orientation.w#q[3] #fake graspable object graspable_object = GraspableObject() graspable_object.reference_frame_id = "/world" mypotentialmodels = DatabaseModelPose() mypotentialmodels.model_id = 18744 mypotentialmodels.confidence = 1.0 mypotentialmodels.pose = copy.deepcopy(initial_pose) graspable_object.potential_models.append(mypotentialmodels) #fake obj world destination pose initial_pose.pose.position.x = 0.43 initial_pose.pose.position.y = 0.0 initial_pose.pose.position.z = 1.088 initial_pose.pose.orientation.x = 0.0 #self.box_pose.pose.orientation.x#q[0] initial_pose.pose.orientation.y = 0.0 #self.box_pose.pose.orientation.y#q[1] initial_pose.pose.orientation.z = 0.567 #self.box_pose.pose.orientation.z#q[2] initial_pose.pose.orientation.w = 0.824 #self.box_pose.pose.orientation.w#q[3] list_of_poses = self.compute_list_of_poses(initial_pose, graspable_object, executed_grasp) #print "list of pose",list_of_poses #self.place_object(self.object.graspable_object, self.object.graspable_object_name, self.object_name, self.list_of_poses) 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 def place_object(self, graspable_object, graspable_object_name, object_name, list_of_poses): """ Place the given object in the given pose """ if self.pickup_result == None: rospy.logwarn( "No objects where picked up. Aborting place object action.") return info_tmp = "Placing " + object_name rospy.loginfo(info_tmp) place_goal = PlaceGoal() #place at the prepared location place_goal.place_locations = list_of_poses place_goal.collision_object_name = graspable_object_name place_goal.collision_support_surface_name = self.gui.collision_support_surface_name print "collision support surface name: ", self.gui.collision_support_surface_name #information about which grasp was executed on the object, #returned by the pickup action place_goal.grasp = self.pickup_result.grasp #use the right rm to place place_goal.arm_name = "right_arm" #padding used when determining if the requested place location #would bring the object in collision with the environment place_goal.place_padding = 0.01 #how much the gripper should retreat after placing the object place_goal.desired_retreat_distance = 0.1 place_goal.min_retreat_distance = 0.05 #we will be putting down the object along the "vertical" direction #which is along the z axis in the fixed frame direction = Vector3Stamped() direction.header.stamp = rospy.get_rostime() direction.header.frame_id = "/world" direction.vector.x = 0 direction.vector.y = 0 direction.vector.z = 1 place_goal.approach.direction = direction place_goal.approach.min_distance = 0.01 place_goal.approach.desired_distance = 0.03 #request a vertical put down motion of 10cm before placing the object place_goal.desired_retreat_distance = 0.1 place_goal.min_retreat_distance = 0.05 #we are not using tactile based placing place_goal.use_reactive_place = False placeresult = self.pickupservice.place(place_goal) '''place_client = actionlib.SimpleActionClient('/object_manipulator/object_manipulator_place', PlaceAction) place_client.wait_for_server() rospy.loginfo("Place server ready") place_client.send_goal(place_goal) #timeout after 1sec #TODO: change this when using the robot place_client.wait_for_result(timeout=rospy.Duration.from_sec(3600.0)) rospy.loginfo("Got Place results") place_result = place_client.get_result() if place_client.get_state() != GoalStatus.SUCCEEDED: rospy.logerr("The place action has failed: " + str(place_result.manipulation_result.value) ) print place_result ''' if placeresult.manipulation_result.value == ManipulationResult.SUCCESS: loginfo("Place succeeded, now retreating") self.pickupservice.retreat(0.07) else: loginfo("Place failed") return 1 return 0 def compute_list_of_poses(self, destination_pose, graspable_object, executed_grasp): list_of_poses = [] # find hand to object transform # executed_grasp is hand pose in world frame # graspable_object is object in world frame hand_world_pose = executed_grasp.grasp_pose #print "hand_world", hand_world_pose object_world_pose = graspable_object.potential_models[0].pose.pose # print "object world" , object_world_pose tmathandworld = pose_to_mat(hand_world_pose) tmatobjworld = pose_to_mat(object_world_pose) hand_object_pose = mat_to_pose( np.dot(np.linalg.inv(tmatobjworld), tmathandworld)) #print "hand_object_pose" , hand_object_pose # generate rotational_symmetric hand to object frames hand_object_poses = self.generate_rotational_symmetric_poses( hand_object_pose, 16) target_pose = PoseStamped() target_pose = copy.deepcopy(destination_pose) target_pose.header.stamp = rospy.get_rostime() #print "target_object_world pose" , target_pose tmattarget_world = pose_to_mat(target_pose.pose) for hand_object_pose in hand_object_poses: tmathandobj = pose_to_mat(hand_object_pose) target_hand_posestamped_world = PoseStamped() target_hand_posestamped_world.header.frame_id = "/world" target_hand_posestamped_world.pose = mat_to_pose( np.dot(tmattarget_world, tmathandobj)) target_hand_posestamped_world.header.stamp = rospy.get_rostime() list_of_poses.append(target_hand_posestamped_world) self.draw_place_area(list_of_poses, graspable_object) return list_of_poses def generate_rotational_symmetric_poses(self, hand_object_pose, step=8): ''' Generate a list of 20 pregrasps around the z axis of the object ''' generated_poses = [] pose = hand_object_pose #We'll generate 20 poses centered in 0,0,0 with orientations in 360 degrees around z for i in range(0, step): angle = (6.28318532 * i) / step q = tf.transformations.quaternion_from_euler(0.0, 0.0, angle) rotation_pose = Pose(Point(0, 0, 0), Quaternion(0.0, 0.0, 0.0, 1.0)) rotation_pose.orientation = Quaternion(*q) tmatrotz = pose_to_mat(rotation_pose) # premultiply by the rotation matrix tmatpose = pose_to_mat(pose) rotated_pose = mat_to_pose(np.dot(tmatrotz, tmatpose)) generated_poses.append(rotated_pose) return generated_poses def draw_place_area(self, list_of_poses, graspable_object): ''' Displays all the possible placing locations that are going to be tried. ''' #try: (trans_palm, rot_palm) = self.listener.lookupTransform('/world', '/palm', rospy.Time(0)) #except: # return for index, pose_stamped in enumerate(list_of_poses): pose_tmp = Pose() pose_tmp.position.x = pose_stamped.pose.position.x pose_tmp.position.y = pose_stamped.pose.position.y pose_tmp.position.z = pose_stamped.pose.position.z pose_tmp.orientation.x = pose_stamped.pose.orientation.x pose_tmp.orientation.y = pose_stamped.pose.orientation.y pose_tmp.orientation.z = pose_stamped.pose.orientation.z pose_tmp.orientation.w = pose_stamped.pose.orientation.w mat = pose_to_mat(pose_tmp) self.draw_functions.draw_rviz_box(mat, [.01, .01, .01], frame='/world', ns='place_' + str(index), id=1000 + index, duration=90, color=[0.5, 0.5, 0.0], opaque=1.0) def call_find_cluster_bounding_box(self, cluster): req = FindClusterBoundingBoxRequest() req.cluster = cluster service_name = "find_cluster_bounding_box" rospy.loginfo("waiting for find_cluster_bounding_box service") rospy.wait_for_service(service_name) rospy.loginfo("service found") serv = rospy.ServiceProxy(service_name, FindClusterBoundingBox) try: res = serv(req) except rospy.ServiceException, e: rospy.logerr("error when calling find_cluster_bounding_box: %s" % e) return 0 if not res.error_code: return (res.pose, res.box_dims) else: return (None, None)
class PickServer(object): def __init__(self): self.draw_functions = draw_functions.DrawFunctions('grasp_markers') self.pickupservice = Execution() self.object = None self.object_name = None self.server = actionlib.SimpleActionServer('pick_object', PickObjectAction, self.execute, False) self.server.start() loginfo("Step actionlib servers: pick server ready") def execute(self, goal): (res, initial_pose, executed_grasp) = self.pick_object( goal.object_to_pick, goal.collision_support_surface_name) if self.server.is_active(): if res == 0: self.server.publish_feedback( PickObjectFeedback(100, "Pickup succeeded")) self.server.set_succeeded( PickObjectResult(res, initial_pose, executed_grasp)) else: rospy.logwarn("Pickup failed") self.server.publish_feedback( PickObjectFeedback(100, "Error: Pickup failed")) self.server.set_succeeded( PickObjectResult(res, initial_pose, executed_grasp)) def pick_object(self, object_to_pick, collision_support_surface_name): self.server.publish_feedback( PickObjectFeedback(1, "Finding object bounding box")) self.object_name = object_to_pick.model_description.name self.object = object_to_pick graspable_object = self.object.graspable_object # draw a bounding box around the selected object (box_pose, box_dims) = self.call_find_cluster_bounding_box( graspable_object.cluster) if box_pose == None: return (1, None, None) box_mat = pose_to_mat(box_pose.pose) rospy.logerr("box_pose %f %f %f q: %f %f %f %f", box_pose.pose.position.x, box_pose.pose.position.y, box_pose.pose.position.z, box_pose.pose.orientation.x, box_pose.pose.orientation.y, box_pose.pose.orientation.z, box_pose.pose.orientation.w) box_ranges = [[-box_dims.x / 2, -box_dims.y / 2, -box_dims.z / 2], [box_dims.x / 2, box_dims.y / 2, box_dims.z / 2]] self.box_pose = box_pose self.draw_functions.draw_rviz_box(box_mat, box_ranges, '/world', ns='bounding box', color=[0, 0, 1], opaque=0.25, duration=60) if self.server.is_preempt_requested(): self.server.publish_feedback( PickObjectFeedback(50, "Pick action cancelled")) self.server.set_preempted() return (1, None, None) self.server.publish_feedback( PickObjectFeedback(10, "Calling pick up service")) # call the pickup service res = self.pickup(graspable_object, self.object.graspable_object_name, self.object_name, collision_support_surface_name) initial_pose = PoseStamped() initial_pose.header.stamp = rospy.get_rostime() initial_pose.header.frame_id = "/world" initial_pose.pose.position.x = self.box_pose.pose.position.x initial_pose.pose.position.y = self.box_pose.pose.position.y initial_pose.pose.position.z = self.box_pose.pose.position.z - box_dims.z / 2 # graspable object is from bottom but bounding box is at center ! initial_pose.pose.orientation.x = self.box_pose.pose.orientation.x initial_pose.pose.orientation.y = self.box_pose.pose.orientation.y initial_pose.pose.orientation.z = self.box_pose.pose.orientation.z initial_pose.pose.orientation.w = self.box_pose.pose.orientation.w if res == 0: #correctly picked up executed_grasp = self.pickup_result.grasp else: executed_grasp = Grasp() return (res, initial_pose, executed_grasp) def pickup(self, graspable_object, graspable_object_name, object_name, collision_support_surface_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 = 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 def call_find_cluster_bounding_box(self, cluster): req = FindClusterBoundingBoxRequest() req.cluster = cluster service_name = "find_cluster_bounding_box" rospy.loginfo("waiting for find_cluster_bounding_box service") rospy.wait_for_service(service_name) rospy.loginfo("service found") serv = rospy.ServiceProxy(service_name, FindClusterBoundingBox) try: res = serv(req) except rospy.ServiceException, e: rospy.logerr("error when calling find_cluster_bounding_box: %s" % e) return 0 if not res.error_code: return (res.pose, res.box_dims) else: return (None, None)