def from_dict(obj_dict): obj = Object3d() obj.id = obj_dict['id'] obj.type = obj_dict['type'] obj.pose = Pose3.from_dict(obj_dict['pose']) obj.bbox = BBox3.from_dict(obj_dict['bounding_box']) return obj
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 convert_ros_msg(msg): '''Converts an 'mcr_perception_msgs.msg.BoundingBox' message to an 'action_execution.geometry.bbox.BBox3' object. Keyword argumens: msg -- an 'mcr_perception_msgs.msg.BoundingBox' message ''' bbox = BBox3() min_vec = Vector3(msg.center.x - (msg.dimensions.x / 2.), msg.center.y - (msg.dimensions.y / 2.), msg.center.z - (msg.dimensions.z / 2.)) max_vec = Vector3(msg.center.x + (msg.dimensions.x / 2.), msg.center.y + (msg.dimensions.y / 2.), msg.center.z + (msg.dimensions.z / 2.)) bbox.min = min_vec bbox.max = max_vec return bbox
def from_dict(obj_dict): obj = Plane() obj.id = obj_dict['id'] obj.pose = Pose3.from_dict(obj_dict['pose']) obj.bbox = BBox3.from_dict(obj_dict['bounding_box']) return obj
def __init__(self, plane_id='', pose=Pose3(), bbox=BBox3()): self.id = plane_id self.pose = deepcopy(pose) self.bbox = deepcopy(bbox)
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))) obj_config.static_objs = [static_obj1, static_obj2, static_obj3]
''' 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(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(pose=Pose3(frame_id='odom', position=Vector3(-0.75, 0.05, 0.75), orientation=Vector3(0., 0., 0.)), bbox=BBox3(Vector3(-0.789, 0.011, 0.75), Vector3(-0.711, 0.089, 0.958))) obj_config.static_objs = [static_obj1, static_obj2] obj_config.manipulated_obj = Object3d( pose=Pose3(frame_id='odom', position=Vector3(-0.126, 0.190, 0.851), orientation=Vector3(0., 0., 0.)), bbox=BBox3(Vector3(-0.164, 0.151, 0.849), Vector3(-0.087, 0.229, 1.057))) obj_config.surface = Plane(pose=Pose3(frame_id='odom', position=Vector3(-1., 0., 0.75)),
def __init__(self, obj_id='', obj_type='', pose=Pose3(), bbox=BBox3()): self.id = obj_id self.type = obj_type self.pose = deepcopy(pose) self.bbox = deepcopy(bbox)
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) diagnoses, experience_correction = execution_model.correct_failed_experience(predicate_library=GraspingPredicateLibrary, state_update_fn=state_update_fn, failure_search_params=failure_search_params, diagnosis_repetition_count=50, diagnosis_candidate_confidence=0.8, correction_sample_count=10, pose=gripper_pose, b_box=handle_bbox) alternative_experience = action_parameters[i] + experience_correction if not np.allclose(experience_correction, np.zeros_like(experience_correction)): original_goal_positions.append(action_parameters[i]) alternative_experiences.append(alternative_experience) print('Sample: {0}'.format(i+1))