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) elif plannerType.lower() == "rrtxstatic": return og.RRTXstatic(si) elif plannerType.lower() == "rrtsharp": return og.RRTsharp(si) # multi-query elif plannerType.lower() == "lazyprmstar": return og.LazyPRMstar(si) # single-query elif plannerType.lower() == "rrtconnect": return og.RRTConnect(si) elif plannerType.lower() == "lbtrrt": return og.LBTRRT(si) elif plannerType.lower() == "lazylbtrrt": return og.LazyLBTRRT(si) else: ou.OMPL_ERROR( "Planner-type is not implemented in allocation function.")
def planBITstar(q_st, q_g, res): # create an SE2 state space space = ob.RealVectorStateSpace(3) # set lower and upper bounds bounds = ob.RealVectorBounds(3) bounds.setLow(0) bounds.setHigh(res) space.setBounds(bounds) # construct an instance of space information from this state space si = ob.SpaceInformation(space) # set state validity checking for this space si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) # create start state start = ob.State(space) start[0] = q_st[0] start[1] = q_st[1] # create goal state goal = ob.State(space) goal[0] = q_g[0] goal[1] = q_g[1] # create a problem instance pdef = ob.ProblemDefinition(si) # set the start and goal states pdef.setStartAndGoalStates(start, goal) # create a planner for the defined space planner = og.BITstar(si) # set the problem we are trying to solve for the planner planner.setProblemDefinition(pdef) # perform setup steps for the planner planner.setup() # attempt to solve the problem within one second of planning time solved = planner.solve(1.0) 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") path_arr = [] print(path.getStateCount()) for i in range(path.getStateCount()): path_arr.append([]) path_arr[i].append(path.getState(i)[0]) path_arr[i].append(path.getState(i)[1]) return path_arr else: print("No solution found") return []
def allocatePlanner(si, plannerType): if plannerType.lower() == "bitstar": return og.BITstar(si) elif plannerType.lower() == "fmtstar": return og.FMT(si) elif plannerType.lower() == "prmstar": return og.PRMstar(si) elif plannerType.lower() == "rrtstar": return og.RRTstar(si) else: OMPL_ERROR("Planner-type is not implemented in allocation function.")
def setPlanner_3d(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: print("Planner-type is not implemented in allocation function.") planner = og.RRTstar(self.si) self.ss.setPlanner(planner)
def allocatePlanner(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() == "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.")
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.BITstar(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")
def plan(self, start_tup, goal_tup, turning_radius=10, planning_time=30.0): def isStateValid(state): y = int(state.getY()) x = int(state.getX()) if y < 0 or x < 0 or y >= self.map_data.shape[ 0] or x >= self.map_data.shape[1]: return False return bool(self.map_data[y, x] == 0) space = ob.DubinsStateSpace(turningRadius=turning_radius) # Set State Space bounds bounds = ob.RealVectorBounds(2) bounds.setLow(0, 0) bounds.setHigh(0, self.map_data.shape[1]) bounds.setLow(1, 0) bounds.setHigh(1, self.map_data.shape[0]) space.setBounds(bounds) si = ob.SpaceInformation(space) si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) si.setStateValidityCheckingResolution( self.params["validity_resolution"] ) # Set based on thinness of walls in map si.setValidStateSamplerAllocator( ob.ValidStateSamplerAllocator( ob.ObstacleBasedValidStateSampler(si))) si.setup() # Set Start and Goal states start = ob.State(space) start().setX(start_tup[0]) start().setY(start_tup[1]) start().setYaw(start_tup[2]) goal = ob.State(space) goal().setX(goal_tup[0]) goal().setY(goal_tup[1]) goal().setYaw(goal_tup[2]) # Set Problem Definition pdef = ob.ProblemDefinition(si) pdef.setStartAndGoalStates(start, goal) optimObj = ob.PathLengthOptimizationObjective(si) pdef.setOptimizationObjective(optimObj) # Set up planner optimizingPlanner = og.BITstar(si) optimizingPlanner.setProblemDefinition(pdef) optimizingPlanner.setup() solved = optimizingPlanner.solve(planning_time) def solution_path_to_tup(solution_path): result = [] states = solution_path.getStates() for state in states: x = state.getX() y = state.getY() yaw = state.getYaw() result.append((x, y, yaw)) return result solutionPath = None if solved: solutionPath = pdef.getSolutionPath() solutionPath.interpolate(self.params['interpolation_density']) solutionPath = solution_path_to_tup(solutionPath) return bool(solved), solutionPath