def extract_predicate_values(self, data_point_count: int, handle_positions: np.ndarray, goal_positions: np.ndarray, motion_durations: np.ndarray) -> np.ndarray: predicate_values = np.zeros( (data_point_count, MetaPredicateData.get_predicate_count(PullingPredicateLibrary) + 1), dtype=int) predicates = MetaPredicateData.get_predicates(PullingPredicateLibrary) for i in range(data_point_count): handle_position = Vector3(x=handle_positions[i, 0], y=handle_positions[i, 1], z=handle_positions[i, 2]) handle_orientation = Vector3(x=0., y=0., z=0.) handle_pose = Pose3(position=handle_position, orientation=handle_orientation) goal_position = Vector3(x=goal_positions[i, 0], y=goal_positions[i, 1], z=goal_positions[i, 2]) goal_orientation = Vector3(x=0., y=0., z=0.) goal_pose = Pose3(position=goal_position, orientation=goal_orientation) for j, p in enumerate(predicates): predicate_values[i, j] = p(handle_pose, goal_pose) predicate_values[i, -1] = PullingPredicateLibrary.qualitative_time( motion_durations[i]) return predicate_values
def extract_predicate_values(self, data_point_count: int, goal_positions: np.ndarray, goal_orientations: np.ndarray, handle_bb_mins: np.ndarray, handle_bb_maxs: np.ndarray) -> np.ndarray: predicate_values = np.zeros( (data_point_count, MetaPredicateData.get_predicate_count(GraspingPredicateLibrary)), dtype=int) predicates = MetaPredicateData.get_predicates(GraspingPredicateLibrary) for i in range(data_point_count): gripper_position = Vector3(x=goal_positions[i, 0], y=goal_positions[i, 1], z=goal_positions[i, 2]) gripper_orientation = Vector3(x=goal_orientations[i, 0], y=goal_orientations[i, 1], z=goal_orientations[i, 2]) gripper_pose = Pose3(position=gripper_position, orientation=gripper_orientation) bb_min = Vector3(x=handle_bb_mins[i, 0], y=handle_bb_mins[i, 1], z=handle_bb_mins[i, 2]) bb_max = Vector3(x=handle_bb_maxs[i, 0], y=handle_bb_maxs[i, 1], z=handle_bb_maxs[i, 2]) handle_bbox = BBox3(min_values=bb_min, max_values=bb_max) for j, p in enumerate(predicates): predicate_values[i, j] = p(gripper_pose, handle_bbox) return predicate_values
def generate_data(self, number_of_samples): '''Generates a set of samples Keyword arguments: number_of_samples -- number of samples to generate Returns: candidate_poses -- a list of 'action_execution.geometry.pose.Pose3' objects ''' candidate_poses = list() # we get the z-projections of the objects that are already on the surface object_polygons = list() for obj in self.objects: polygon = obj.get_z_projection() object_polygons.append(polygon) # we generate samples in the free space on the surface, i.e. # we ignore samples that cause collisions with the other objects collected_samples = 0 distances = np.zeros(number_of_samples) while collected_samples < number_of_samples: obj_copy = deepcopy(self.manipulated_object) # we generate a random position on the surface position = Vector2.random_vector(self.surface.bbox.min, self.surface.bbox.max) obj_copy.planar_translate_to(position) # we generate a random orientation around z; # we don't rotate the object around the other axes z_orientation = np.random.uniform(0., 2.*np.pi) obj_copy.rotate_around_z(z_orientation) # we take the generated pose as a valid candidate if the # object doesn't collide with any of the objects on the surface position = Vector3(position.x, position.y, self.surface.pose.position.z) orientation = Vector3(obj_copy.pose.orientation.x, obj_copy.pose.orientation.y, z_orientation) pose = Pose3(self.manipulated_object.pose.frame_id, position, orientation) candidate_poses.append(pose) distances[collected_samples] = position.distance(self.robot_pose.position) collected_samples += 1 # we value closer poses higher than farther ones; # the results are treated as success probabilities, # though the values actually encode preference success_probabilities = np.max(distances) / distances success_probabilities /= np.sum(success_probabilities) return {'candidate_poses': candidate_poses, 'success_probabilities': success_probabilities}
def __init__(self, frame_id='', position=Vector3(), orientation=Vector3()): '''Keyword arguments: position -- an 'action_execution.geometry.vector.Vector3' object orientation -- an 'action_execution.geometry.vector.Vector3' object ''' self.frame_id = frame_id self.position = deepcopy(position) self.orientation = deepcopy(orientation)
def generate_data(self, number_of_samples): '''Generates a set of samples Keyword arguments: number_of_samples -- number of samples to generate Returns: candidate_poses -- a list of 'action_execution.geometry.pose.Pose3' objects ''' candidate_poses = list() # we generate samples over the target object collected_samples = 0 while collected_samples < number_of_samples: obj_copy = deepcopy(self.manipulated_object) # we generate a random 2D position over the target object position = Vector2.random_vector(self.target_object.bbox.min, self.target_object.bbox.max) obj_copy.planar_translate_to(position) # we generate a random orientation around z; # we don't rotate the object around the other axes z_orientation = np.random.uniform(0., 2.*np.pi) obj_copy.rotate_around_z(z_orientation) position = Vector3(position.x, position.y, self.target_object.bbox.max.z) orientation = Vector3(obj_copy.pose.orientation.x, obj_copy.pose.orientation.y, z_orientation) pose = Pose3(self.manipulated_object.pose.frame_id, position, orientation) candidate_poses.append(pose) collected_samples += 1 success_probabilities = np.ones(number_of_samples) / (number_of_samples * 1.) return {'candidate_poses': candidate_poses, 'success_probabilities': success_probabilities}
def get_coordinates(self): '''Returns a list of 'action_execution.geometry.vector.Vector3' objects that represent the coordinates of the bounding box. ''' point1 = Vector3(self.min.x, self.min.y, self.min.z) point2 = Vector3(self.min.x, self.min.y, self.max.z) point3 = Vector3(self.min.x, self.max.y, self.min.z) point4 = Vector3(self.min.x, self.max.y, self.max.z) point5 = Vector3(self.max.x, self.min.y, self.min.z) point6 = Vector3(self.max.x, self.min.y, self.max.z) point7 = Vector3(self.max.x, self.max.y, self.min.z) point8 = Vector3(self.max.x, self.max.y, self.max.z) points = [ point1, point2, point3, point4, point5, point6, point7, point8 ] return points
def generate_data(self, number_of_samples): '''Generates a set of samples Keyword arguments: number_of_samples -- number of samples to generate Returns: candidate_poses -- a list of 'action_execution.geometry.pose.Pose3' objects ''' candidate_poses = list() # we get the z-projections of the objects that are already on the surface object_polygons = list() for obj in self.objects: polygon = obj.get_z_projection() object_polygons.append(polygon) # we generate samples in the free space on the surface, i.e. # we ignore samples that cause collisions with the other objects collected_samples = 0 while collected_samples < number_of_samples: obj_copy = deepcopy(self.manipulated_object) # we generate a random position on the surface position = Vector2.random_vector(self.surface.bbox.min, self.surface.bbox.max) obj_copy.planar_translate_to(position) # we generate a random orientation around z; # we don't rotate the object around the other axes z_orientation = np.random.uniform(0., 2. * np.pi) obj_copy.rotate_around_z(z_orientation) # we check if the object would collide with any of the other # objects in the newly generated pose manipulated_object_polygon = obj_copy.get_z_projection() collision = False for obj in object_polygons: if obj.intersects(manipulated_object_polygon): collision = True break # we take the generated pose as a valid candidate if the # object doesn't collide with any of the objects on the surface if not collision: position = Vector3(position.x, position.y, self.surface.pose.position.z) orientation = Vector3(obj_copy.pose.orientation.x, obj_copy.pose.orientation.y, z_orientation) pose = Pose3(self.manipulated_object.pose.frame_id, position, orientation) candidate_poses.append(pose) collected_samples += 1 success_probabilities = np.ones(number_of_samples) / ( number_of_samples * 1.) return { 'candidate_poses': candidate_poses, 'success_probabilities': success_probabilities }
cmap_factor = 100. colormap = plt.cm.RdYlGn(probs * cmap_factor)[:, 0:3] positions = np.zeros((len(poses), 2)) for i, pose in enumerate(poses): positions[i] = np.array([pose.position.x, pose.position.y]) plt.scatter(positions[:, 0], positions[:, 1], c=colormap, zorder=1000) plt.xlim([surface_coords[0] - 1., surface_coords[0] + surface_width + 1.]) plt.ylim([surface_coords[1] - 1., surface_coords[1] + surface_height + 1.]) plt.show() if __name__ == '__main__': robot_pose = Pose3(frame_id='odom_combined', position=Vector3(0.85, 0., 0.), orientation=Vector3(0., 0., 0.)) model = Proximity(frame_id=obj_config.frame_id, manipulated_object=obj_config.manipulated_obj, objects_on_surface=obj_config.static_objs, surface=obj_config.surface, robot_pose=robot_pose) number_of_samples = 1000 model_results = model.generate_data(number_of_samples) poses = model_results['candidate_poses'] probs = model_results['success_probabilities'] plot_points(obj_config.surface, obj_config.static_objs, poses, probs) timestamp = int(round(time.time()) * 1000)
def __init__(self, min_values=Vector3(), max_values=Vector3()): self.min = deepcopy(min_values) self.max = deepcopy(max_values)
along with action-execution. If not, see <http://www.gnu.org/licenses/>. ''' from action_execution.geometry.vector import Vector3 from action_execution.geometry.bbox import BBox3 from action_execution.geometry.pose import Pose3 from action_execution.geometry.object import Object3d from action_execution.geometry.plane import Plane from action_execution.utils.configuration import Configuration obj_config = Configuration() obj_config.frame_id = 'odom' static_obj1 = Object3d(obj_type='ketchup_bottle', pose=Pose3(frame_id='odom', position=Vector3(-0.75, -0.25, 0.75), orientation=Vector3(0., 0., 0.)), bbox=BBox3(Vector3(-0.789, -0.289, 0.75), Vector3(-0.711, -0.211, 0.958))) static_obj2 = Object3d(obj_type='ketchup_bottle', pose=Pose3(frame_id='odom', position=Vector3(-1., -0.1, 0.75), orientation=Vector3(0., 0., 0.)), bbox=BBox3(Vector3(-1.039, -0.139, 0.75), Vector3(-0.961, -0.061, 0.958))) static_obj3 = Object3d(obj_type='book', pose=Pose3(frame_id='odom', position=Vector3(-1., 0.5, 0.75), orientation=Vector3(0., 0., 1.57)), bbox=BBox3(Vector3(-1.225, 0.635, 0.75), Vector3(-0.775, 0.365, 0.798)))
def generate_data(self, number_of_samples): '''Generates a set of samples Keyword arguments: number_of_samples -- number of samples to generate Returns: candidate_poses -- a list of 'action_execution.geometry.pose.Pose3' objects ''' candidate_poses = list() # we get the z-projections of the objects that are already on the surface object_polygons = list() for obj in self.objects: polygon = obj.get_z_projection() object_polygons.append(polygon) # we generate samples that are closest to objects of the same type, i.e. # we ignore samples that are closest to objects of other types collected_samples = 0 distances = np.zeros(number_of_samples) while collected_samples < number_of_samples: obj_copy = deepcopy(self.manipulated_object) # we generate a random position on the surface position = Vector2.random_vector(self.surface.bbox.min, self.surface.bbox.max) obj_copy.planar_translate_to(position) # we generate a random orientation around z; # we don't rotate the object around the other axes z_orientation = np.random.uniform(0., 2.*np.pi) obj_copy.rotate_around_z(z_orientation) position = Vector3(position.x, position.y, self.surface.pose.position.z) orientation = Vector3(obj_copy.pose.orientation.x, obj_copy.pose.orientation.y, z_orientation) obj_distances = np.zeros(len(self.objects)) for i, obj in enumerate(self.objects): obj_distances[i] = position.distance(obj.pose.position) min_dist_idx = np.argmin(obj_distances) # TODO: use a knowledge base to replace the equality check with a # broader test about the object categories if self.object_types[min_dist_idx] == self.manipulated_object.type: pose = Pose3(self.manipulated_object.pose.frame_id, position, orientation) candidate_poses.append(pose) distances[collected_samples] = obj_distances[min_dist_idx] collected_samples += 1 # we value closer poses higher than farther ones; # the results are treated as success probabilities, # though the values actually encode preference success_probabilities = np.max(distances) / distances success_probabilities /= np.sum(success_probabilities) return {'candidate_poses': candidate_poses, 'success_probabilities': success_probabilities}
handle_size_y = np.mean(handle_bb_maxs[:,1] - handle_bb_mins[:,1]) handle_size_z = np.mean(handle_bb_maxs[:,2] - handle_bb_mins[:,2]) failure_search_params = FailureSearchParams(parameter_stds=np.array([handle_size_x/10, handle_size_y/10, handle_size_z/10]), max_sample_count=200, range_increase_percentage=5) print('Diagnosing failed executions and suggesting corrections...') original_goal_positions = [] alternative_experiences = [] for i in range(goal_positions.shape[0]): if labels[i] == 1: continue gripper_position = Vector3(x=goal_positions[i,0], y=goal_positions[i,1], z=goal_positions[i,2]) gripper_orientation = Vector3(x=goal_orientations[i,0], y=goal_orientations[i,1], z=goal_orientations[i,2]) gripper_pose = Pose3(position=gripper_position, orientation=gripper_orientation) bb_min = Vector3(x=handle_bb_mins[i,0], y=handle_bb_mins[i,1], z=handle_bb_mins[i,2]) bb_max = Vector3(x=handle_bb_maxs[i,0], y=handle_bb_maxs[i,1], z=handle_bb_maxs[i,2])