Beispiel #1
0
  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, 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
Beispiel #3
0
    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
Beispiel #5
0
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 preprocess_edge(oracle, edge):
  if not hasattr(edge, 'preprocessed'):
    edge.preprocessed = True
    with oracle.robot_saver():
      CSpace.robot_arm_and_base(oracle.robot.GetActiveManipulator()).set_active()
      for q in edge.configs():
        if not hasattr(q, 'saver'):
          oracle.robot.SetActiveDOFValues(q.value)
          q.saver = oracle.robot_saver()
          q.manip_trans = get_manip_trans(oracle)
          q.aabbs = [(aabb_min(aabb), aabb_max(aabb)) for aabb in [oracle.compute_part_aabb(part) for part in USE_ROBOT_PARTS]]
def preprocess_edge(oracle, edge):
    if not hasattr(edge, 'preprocessed'):
        edge.preprocessed = True
        with oracle.robot_saver():
            CSpace.robot_arm_and_base(
                oracle.robot.GetActiveManipulator()).set_active()
            for q in edge.configs():
                if not hasattr(q, 'saver'):
                    oracle.robot.SetActiveDOFValues(q.value)
                    q.saver = oracle.robot_saver()
                    q.manip_trans = get_manip_trans(oracle)
                    q.aabbs = [(aabb_min(aabb), aabb_max(aabb)) for aabb in [
                        oracle.compute_part_aabb(part)
                        for part in USE_ROBOT_PARTS
                    ]]
Beispiel #8
0
def preprocess_trajs(oracle, start_config, trajs): # TODO - should store start_config, trajs tuple
  if any(not hasattr(traj, 'preprocessed') for traj in trajs):
    with oracle.robot_saver():
      set_full_config(oracle.robot, start_config.value) # TODO - get rid of this or something
      for traj in trajs:
        traj.preprocessed = True
        traj.cspace.set_active()
        traj.savers = [] # TODO - nasty
        traj.manip_trans = []
        traj.aabbs = []
        for q in traj.path():
          traj.cspace.body.SetActiveDOFValues(q)
          traj.savers.append(oracle.robot_saver())
          traj.manip_trans.append(get_manip_trans(oracle))
          traj.aabbs.append([(aabb_min(aabb), aabb_max(aabb)) for aabb in [oracle.compute_part_aabb(part) for part in USE_ROBOT_PARTS]])
Beispiel #9
0
  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
Beispiel #10
0
def preprocess_trajs(oracle, start_config,
                     trajs):  # TODO - should store start_config, trajs tuple
    if any(not hasattr(traj, 'preprocessed') for traj in trajs):
        with oracle.robot_saver():
            set_full_config(
                oracle.robot,
                start_config.value)  # TODO - get rid of this or something
            for traj in trajs:
                traj.preprocessed = True
                traj.cspace.set_active()
                traj.savers = []  # TODO - nasty
                traj.manip_trans = []
                traj.aabbs = []
                for q in traj.path():
                    traj.cspace.body.SetActiveDOFValues(q)
                    traj.savers.append(oracle.robot_saver())
                    traj.manip_trans.append(get_manip_trans(oracle))
                    traj.aabbs.append([(aabb_min(aabb), aabb_max(aabb))
                                       for aabb in [
                                           oracle.compute_part_aabb(part)
                                           for part in USE_ROBOT_PARTS
                                       ]])