class ProbabilisticPointClusterGraspPlanner(PointClusterGraspPlanner): def __init__(self, tf_listener = None, tf_broadcaster = None): PointClusterGraspPlanner.__init__(self) self.draw_box = True self.cbbf = ProbabilisticClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster) ##draw the fitted box model of the object, placed at identy pose (4x4 scipy matrix) in frame (defaults to the object frame) def draw_bounding_box(self, ranges, pause_after_broadcast = 0, name = 'deterministic_object_box', color=[0,0.5,1]): #broadcast the gripper frame to tf (object_frame_pos, object_frame_quat) = mat_to_pos_and_quat(self.object_to_cluster_frame) self.tf_broadcaster.sendTransform(object_frame_pos, object_frame_quat, rospy.Time.now(), "object_frame", self.cluster_frame) now = rospy.Time.now() if pause_after_broadcast: time.sleep(.1) pose_mat = scipy.matrix(scipy.identity(4)) # z offset pose_mat[2,3] = ranges[2]/2 self.draw_functions.draw_rviz_box(pose_mat, ranges, frame = 'object_frame', ns = name, \ id = 0, color = color, duration = 100, opaque = .7) ##initialization for planning grasps def init_cluster_grasper(self, cluster, probabilities=[], use_probability=True): self.cluster_frame = cluster.header.frame_id #use PCA to find the object frame and bounding box dims, and to get the cluster points in the object frame (z=0 at bottom of cluster) if use_probability: if len(probabilities) == 0: (self.object_points, self.object_bounding_box_dims, self.object_bounding_box, \ self.object_to_base_frame, self.object_to_cluster_frame) = \ self.cbbf.find_object_frame_and_bounding_box(cluster, []) #empty probabilities if self.draw_box: self.draw_bounding_box(self.object_bounding_box_dims, name='probabilistic_object_box', color=[0.2,0.2,1]) print 'probabilistic_object_box' else: (self.object_points, self.object_bounding_box_dims, self.object_bounding_box, \ self.object_to_base_frame, self.object_to_cluster_frame) = \ self.cbbf.find_object_frame_and_bounding_box(cluster, probabilities) #draw the bounding box if self.draw_box: self.draw_bounding_box(self.object_bounding_box_dims, name='probabilistic_weighted_object_box', color=[0,0.9,0.9]) print 'probabilistic_weighted_object_box' else: (self.object_points, self.object_bounding_box_dims, self.object_bounding_box, \ self.object_to_base_frame, self.object_to_cluster_frame) = \ self.cbbf.find_object_frame_and_bounding_box(cluster, probabilities) if self.draw_box: self.draw_bounding_box(self.object_bounding_box_dims, name='deterministic_object_box', color=[1,0,0]) print 'deterministic_object_box' # MoveIt Stuff: CollisionObject ''' print "self.object_bounding_box_dims=", self.object_bounding_box_dims print "self.object_bounding_box=", self.object_bounding_box print "self.object_to_base_frame.shape=", self.object_to_base_frame.shape print "self.object_to_base_frame=", self.object_to_base_frame print "self.object_to_cluster_frame=", self.object_to_cluster_frame ''' # Add the bounding box as a CollisionObject co = CollisionObject() co.header.stamp = rospy.get_rostime() #co.header.frame_id = "base_footprint" co.header.frame_id = "base_link" co.primitives.append(SolidPrimitive()) co.primitives[0].type = SolidPrimitive.BOX # Clear the previous CollisionObject co.id = "part" co.operation = co.REMOVE self.pub_co.publish(co) # Clear the previously attached object aco = AttachedCollisionObject() aco.object = co self.pub_aco.publish(aco) # Add the new CollisionObject box_height = self.object_bounding_box_dims[2] co.operation = co.ADD co.primitives[0].dimensions.append(self.object_bounding_box_dims[0]) co.primitives[0].dimensions.append(self.object_bounding_box_dims[1]) co.primitives[0].dimensions.append(self.object_bounding_box_dims[2]) co.primitive_poses.append(Pose()) co.primitive_poses[0].position.x = self.object_to_base_frame[0,3] co.primitive_poses[0].position.y = self.object_to_base_frame[1,3] co.primitive_poses[0].position.z = self.object_to_base_frame[2,3] + box_height/2 quat = tf.transformations.quaternion_about_axis(math.atan2(self.object_to_base_frame[1,0], self.object_to_base_frame[0,0]), (0,0,1)) #quat = tf.transformations.quaternion_from_matrix(self.object_to_base_frame) co.primitive_poses[0].orientation.x = quat[0] co.primitive_poses[0].orientation.y = quat[1] co.primitive_poses[0].orientation.z = quat[2] co.primitive_poses[0].orientation.w = quat[3] self.pub_co.publish(co) # END MoveIt! stuff #for which directions does the bounding box fit within the hand? gripper_space = [self.gripper_opening - self.object_bounding_box_dims[i] for i in range(3)] self._box_fits_in_hand = [gripper_space[i] > 0 for i in range(3)] #only half benefit for the longer dimension, if one is significantly longer than the other if self._box_fits_in_hand[0] and self._box_fits_in_hand[1]: if gripper_space[0] > gripper_space[1] and self.object_bounding_box_dims[0]/self.object_bounding_box_dims[1] < .8: self._box_fits_in_hand[1] *= .5 elif gripper_space[1] > gripper_space[0] and self.object_bounding_box_dims[1]/self.object_bounding_box_dims[0] < .8: self._box_fits_in_hand[0] *= .5 #rospy.loginfo("bounding box dims: "+pplist(self.object_bounding_box_dims)) #rospy.loginfo("self._box_fits_in_hand: "+str(self._box_fits_in_hand)) #compute the average number of points per sq cm of bounding box surface (half the surface only) bounding_box_surf = (self.object_bounding_box_dims[0]*100 * self.object_bounding_box_dims[1]*100) + \ (self.object_bounding_box_dims[0]*100 * self.object_bounding_box_dims[2]*100) + \ (self.object_bounding_box_dims[1]*100 * self.object_bounding_box_dims[2]*100) self._points_per_sq_cm = self.object_points.shape[1] / bounding_box_surf
def __init__(self, tf_listener = None, tf_broadcaster = None): PointClusterGraspPlanner.__init__(self) self.draw_box = True self.cbbf = ProbabilisticClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster)