def fn(self, edge, object_name, object_pose): oracle = self.oracle preprocess_edge(oracle, edge) 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) configs = [ q for q in edge.configs() if fast_aabb_overlap(object_aabb_min, object_aabb_max, q.aabbs) ] if len(configs) == 0: return False #distance = oracle.get_radius2('pr2') + oracle.get_radius2(object_name) #z = get_point(oracle.robot)[-1] #configs = [q for q in edge.configs() if length2(np.array([q.value[-3], q.value[-2], z]) - point_from_pose(object_pose.value)) <= distance] #if len(configs) == 0: return False # TODO - oracle.robot.SetActiveDOFValues(q.value) is the bottleneck: check each object at the same time with oracle.body_saver(object_name): oracle.set_pose(object_name, object_pose) with oracle.robot_saver(): CSpace.robot_arm_and_base( oracle.robot.GetActiveManipulator()).set_active() with collision_saver( oracle.env, openravepy_int.CollisionOptions.ActiveDOFs ): # TODO - does this really do what I think? for q in configs: # TODO - do this with other other collision functions q.saver.Restore() if robot_collision(oracle, object_name): return True return False
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, start_config, trajs, object_name, object_pose): oracle = self.oracle preprocess_trajs(oracle, start_config, trajs) 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) savers = [ traj.savers[i] for traj in trajs for i, aabbs in enumerate(traj.aabbs) if fast_aabb_overlap(object_aabb_min, object_aabb_max, aabbs) ] if len(savers) == 0: return False with oracle.body_saver(object_name): oracle.set_pose(object_name, object_pose) with oracle.robot_saver(): trajs[0].cspace.set_active() with collision_saver( oracle.env, openravepy_int.CollisionOptions.ActiveDOFs): for saver in savers: saver.Restore() if robot_collision(oracle, object_name): return True return False
def fn(self, edge, object_name, object_pose): oracle = self.oracle preprocess_edge(oracle, edge) 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) configs = [q for q in edge.configs() if fast_aabb_overlap(object_aabb_min, object_aabb_max, q.aabbs)] if len(configs) == 0: return False #distance = oracle.get_radius2('pr2') + oracle.get_radius2(object_name) #z = get_point(oracle.robot)[-1] #configs = [q for q in edge.configs() if length2(np.array([q.value[-3], q.value[-2], z]) - point_from_pose(object_pose.value)) <= distance] #if len(configs) == 0: return False # TODO - oracle.robot.SetActiveDOFValues(q.value) is the bottleneck: check each object at the same time with oracle.body_saver(object_name): oracle.set_pose(object_name, object_pose) with oracle.robot_saver(): CSpace.robot_arm_and_base(oracle.robot.GetActiveManipulator()).set_active() with collision_saver(oracle.env, openravepy_int.CollisionOptions.ActiveDOFs): # TODO - does this really do what I think? for q in configs: # TODO - do this with other other collision functions q.saver.Restore() if robot_collision(oracle, object_name): return True return False
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, start_config, trajs, object_name, object_pose): oracle = self.oracle preprocess_trajs(oracle, start_config, trajs) 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) savers = [traj.savers[i] for traj in trajs for i, aabbs in enumerate(traj.aabbs) if fast_aabb_overlap(object_aabb_min, object_aabb_max, aabbs)] if len(savers) == 0: return False with oracle.body_saver(object_name): oracle.set_pose(object_name, object_pose) with oracle.robot_saver(): trajs[0].cspace.set_active() with collision_saver(oracle.env, openravepy_int.CollisionOptions.ActiveDOFs): for saver in savers: saver.Restore() if robot_collision(oracle, object_name): return True return False