Example #1
0
def allocatePlanner(si, plannerType):
    if plannerType.lower() == "bfmtstar":
        return og.BFMT(si)
    elif plannerType.lower() == "bitstar":
        return og.BITstar(si)
    elif plannerType.lower() == "fmtstar":
        return og.FMT(si)
    elif plannerType.lower() == "informedrrtstar":
        return og.InformedRRTstar(si)
    elif plannerType.lower() == "prmstar":
        return og.PRMstar(si)
    elif plannerType.lower() == "rrtstar":
        return og.RRTstar(si)
    elif plannerType.lower() == "sorrtstar":
        return og.SORRTstar(si)
    else:
        ou.OMPL_ERROR(
            "Planner-type is not implemented in allocation function.")
Example #2
0
 def setPlanner(self):
     self.si = self.ss.getSpaceInformation()
     if self.plannerType.lower() == "bitstar":
         planner = og.BITstar(self.si)
     elif self.plannerType.lower() == "fmtstar":
         planner = og.FMT(self.si)
     elif self.plannerType.lower() == "informedrrtstar":
         planner = og.InformedRRTstar(self.si)
     elif self.plannerType.lower() == "prmstar":
         planner = og.PRMstar(self.si)
     elif self.plannerType.lower() == "rrtstar":
         planner = og.RRTstar(self.si)
     elif self.plannerType.lower() == "sorrtstar":
         planner = og.SORRTstar(self.si)
     else:
         OMPL_ERROR(
             "Planner-type is not implemented in allocation function.")
         planner = og.RRTstar(self.si)
     self.ss.setPlanner(planner)
Example #3
0
 def choose_planner(self, si, plannerType):
     if plannerType.lower() == "bfmtstar":
         return og.BFMT(si)
     elif plannerType.lower() == "bitstar":
         return og.BITstar(si)
     elif plannerType.lower() == "fmtstar":
         return og.FMT(si)
     elif plannerType.lower() == "informedrrtstar":
         return og.InformedRRTstar(si)
     elif plannerType.lower() == "prmstar":
         return og.PRMstar(si)
     elif plannerType.lower() == "rrtconnect":
         return og.RRTConnect(si)
     elif plannerType.lower() == "rrtsharp":
         return og.RRTsharp(si)
     elif plannerType.lower() == "rrtstar":
         return og.RRTstar(si)
     elif plannerType.lower() == "sorrtstar":
         return og.SORRTstar(si)
     else:
         OMPL_ERROR("Planner-type is not implemented in allocation function.")
Example #4
0
def allocatePlanner(si, plannerType):
    if plannerType.lower() == "bfmtstar":
        return og.BFMT(si)
    elif plannerType.lower() == "bitstar":
        planner = og.BITstar(si)
        planner.setPruning(False)
        planner.setSamplesPerBatch(200)
        planner.setRewireFactor(20.)
        return planner
    elif plannerType.lower() == "fmtstar":
        return og.FMT(si)
    elif plannerType.lower() == "informedrrtstar":
        return og.InformedRRTstar(si)
    elif plannerType.lower() == "prmstar":
        return og.PRMstar(si)
    elif plannerType.lower() == "rrtstar":
        return og.RRTstar(si)
    elif plannerType.lower() == "sorrtstar":
        return og.SORRTstar(si)
    elif plannerType.lower() == 'rrtconnect':
        return og.RRTConnect(si)
    else:
        ou.OMPL_ERROR(
            "Planner-type is not implemented in allocation function.")
def plan(grid):

    #agent and goal are represented by a point(x,y) and radius
    global x
    global time
    x = grid
    agent: Agent = grid.agent
    goal: Goal = grid.goal

    # Construct the robot state space in which we're planning. R2
    space = ob.RealVectorStateSpace(2)

    # Set the bounds of space to be inside Map
    bounds = ob.RealVectorBounds(2)

    # projection
    pj = ProjectionEvaluator(space)
    print('pj=', pj)

    pj.setCellSizes(list2vec([1.0, 1.0]))
    space.registerDefaultProjection(pj)

    # Construct the robot state space in which we're planning.
    bounds.setLow(0, 0)  #set min x to _    0
    bounds.setHigh(0, grid.size.width)  #set max x to _    x width
    bounds.setLow(1, 0)  #set min y to _    0
    bounds.setHigh(1, grid.size.height)  #set max y to _    y height

    space.setBounds(bounds)

    print("bounds=", bounds.getVolume())

    # Construct a space information instance for this state space
    si = ob.SpaceInformation(space)

    # Set the object used to check which states in the space are valid
    si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))

    # Set robot's starting state to agent's position (x,y) -> e.g. (0,0)
    start = ob.State(space)
    start[0] = float(agent.position.x)
    start[1] = float(agent.position.y)

    print(start[0], start[1])
    # Set robot's goal state (x,y) -> e.g. (1.0,0.0)
    goal = ob.State(space)
    goal[0] = float(grid.goal.position.x)
    goal[1] = float(grid.goal.position.y)

    # Create a problem instance
    pdef = ob.ProblemDefinition(si)

    # Set the start and goal states
    pdef.setStartAndGoalStates(start, goal)

    # Create the optimization objective specified by our command-line argument.
    # This helper function is simply a switch statement.
    #pdef.setOptimizationObjective(allocateObjective(si, objectiveType))

    # ******create a planner for the defined space
    planner = og.PRMstar(si)

    # set the problem we are trying to solve for the planner
    planner.setProblemDefinition(pdef)

    print(planner)
    #print('checking projection',planner.getProjectionEvaluator())

    print("__________________________________________________________")
    # perform setup steps for the planner
    planner.setup()

    # print the settings for this space
    print("space settings\n")
    print(si.settings())

    print("****************************************************************")
    print("problem settings\n")
    # print the problem settings
    print(pdef)

    # attempt to solve the problem within ten second of planning time
    solved = planner.solve(time)

    # For troubleshooting
    if solved:
        # get the goal representation from the problem definition (not the same as the goal state)
        # and inquire about the found path
        path = pdef.getSolutionPath()
        print("Found solution:\n%s" % path)
        #return trace for _find_path_internal method
        print(
            "$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$"
        )

        if path is None:
            return None

        x = pdef.getSolutionPath().printAsMatrix()
        lst = x.split()
        lst = [int(round(float(x), 0)) for x in lst]

        print(x)
        print(lst)

        trace = []
        for i in range(0, len(lst), 2):
            trace.append(Point(lst[i], lst[i + 1]))
            print(trace)
        return trace
    else:
        print("No solution found")
        return None