def fn(self, config, object_name, object_pose, holding): oracle = self.oracle if not hasattr(config, 'preprocessed'): config.processed = True with oracle.robot_saver(): oracle.set_robot_config(config) config.saver = oracle.robot_saver() config.manip_trans = get_manip_trans(oracle) config.aabbs = [(aabb_min(aabb), aabb_max(aabb)) for aabb in [oracle.compute_part_aabb(part) for part in USE_ROBOT_PARTS]] trans = object_trans_from_manip_trans(config.manip_trans, holding.grasp.grasp_trans) distance = oracle.get_radius2(object_name) + oracle.get_radius2(holding.object_name) check_holding = length2(point_from_trans(trans) - point_from_pose(object_pose.value)) <= distance object_aabb = oracle.get_aabb(object_name, trans_from_pose(object_pose.value)) object_aabb_min, object_aabb_max = aabb_min(object_aabb), aabb_max(object_aabb) check_robot = fast_aabb_overlap(object_aabb_min, object_aabb_max, config.aabbs) if not check_holding or not check_robot: return False with oracle.body_saver(object_name): oracle.set_pose(object_name, object_pose) if check_robot: with oracle.robot_saver(): with collision_saver(oracle.env, openravepy_int.CollisionOptions.ActiveDOFs): config.saver.Restore() if robot_collision(oracle, object_name): return True if check_holding: with oracle.body_saver(holding.object_name): set_trans(oracle.get_body(holding.object_name), trans) if collision(oracle, object_name, holding.object_name): return True return False
def fn(self, body_name1, pose1, body_name2, pose2): oracle = self.oracle if not oracle.necessary_collision(body_name1, pose1, body_name2, pose2): return False with oracle.body_saver(body_name1): with oracle.body_saver(body_name2): oracle.set_pose(body_name1, pose1) oracle.set_pose(body_name2, pose2) return collision(oracle, body_name1, body_name2)
def fn(self, body_name, pose, grasp): oracle = self.oracle manip_trans, vector = manip_from_pose_grasp(oracle, pose, grasp) body_trans, gripper_trans = trans_from_pose(pose.value), gripper_trans_from_manip_trans(oracle, manip_trans) distance, direction = length(vector), normalize(vector) steps = int(max(ceil(distance/MIN_DELTA), 1)) + 1 with oracle.robot: with oracle.body_saver(body_name): for dist in (s*distance/steps for s in reversed(range(steps))): set_trans(oracle.get_body(body_name), vector_trans(body_trans, dist*direction)) set_trans(oracle.gripper, vector_trans(gripper_trans, dist*direction)) if any(gripper_collision(oracle, obst_name) for obst_name in oracle.obstacles) or \ any(collision(oracle, body_name, obst_name) for obst_name in oracle.obstacles): return True return False
def fn(self, start_config, trajs, object_name, object_pose, holding): oracle = self.oracle if oracle.traj_collision(start_config, trajs, object_name, object_pose): return True all_trans = [object_trans_from_manip_trans(manip_trans, holding.grasp.grasp_trans) for traj in trajs for manip_trans in traj.manip_trans] # TODO - cache these transforms distance = oracle.get_radius2(object_name) + oracle.get_radius2(holding.object_name) manip_trans = [trans for trans in all_trans if length2(point_from_trans(trans) - point_from_pose(object_pose.value)) <= distance] if len(manip_trans) == 0: return False with oracle.body_saver(object_name): oracle.set_pose(object_name, object_pose) with oracle.body_saver(holding.object_name): for trans in manip_trans: set_trans(oracle.get_body(holding.object_name), trans) if collision(oracle, object_name, holding.object_name): return True return False
def fn(self, edge, object_name, object_pose, holding): oracle = self.oracle preprocess_edge(oracle, edge) all_trans = [object_trans_from_manip_trans(q.manip_trans, holding.grasp.grasp_trans) for q in edge.configs()] # TODO - cache these transforms distance = oracle.get_radius2(object_name) + oracle.get_radius2(holding.object_name) nearby_trans = [trans for trans in all_trans if length2(point_from_trans(trans) - point_from_pose(object_pose.value)) <= distance] if len(nearby_trans) == 0: return False with oracle.body_saver(object_name): oracle.set_pose(object_name, object_pose) with oracle.body_saver(holding.object_name): for trans in nearby_trans: set_trans(oracle.get_body(holding.object_name), trans) if collision(oracle, object_name, holding.object_name): return True return False
def fn(self, body_name1, pose1, grasp, body_name2, pose2): oracle = self.oracle manip_trans, vector = manip_from_pose_grasp(oracle, pose1, grasp) distance, direction = length(vector), normalize(vector) steps = int(max(ceil(distance/MIN_DELTA), 1)) + 1 poses = [Pose(pose_from_trans(vector_trans(trans_from_pose(pose1.value), direction*s*distance/steps))) for s in reversed(range(steps))] # USE OBJECT COLLISION CACHE? if not any(oracle.necessary_collision(body_name1, pose, body_name2, pose2) for pose in poses): return False with oracle.body_saver(body_name1): with oracle.body_saver(body_name2): oracle.set_pose(body_name2, pose2) for pose in poses: oracle.set_pose(body_name1, pose) if collision(oracle, body_name1, body_name2): # TODO - consider using gripper as well return True return False