def plan(samplerIndex): # construct the state space we are planning in space = ob.RealVectorStateSpace(3) # set the bounds bounds = ob.RealVectorBounds(3) bounds.setLow(-1) bounds.setHigh(1) space.setBounds(bounds) # define a simple setup class ss = og.SimpleSetup(space) # set state validity checking for this space ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) # create a start state start = ob.State(space) start[0] = 0 start[1] = 0 start[2] = 0 # create a goal state goal = ob.State(space) goal[0] = 0 goal[1] = 0 goal[2] = 1 # set the start and goal states; ss.setStartAndGoalStates(start, goal) # set sampler (optional; the default is uniform sampling) si = ss.getSpaceInformation() if samplerIndex == 1: # use obstacle-based sampling si.setValidStateSamplerAllocator( ob.ValidStateSamplerAllocator(allocOBValidStateSampler)) elif samplerIndex == 2: # use my sampler si.setValidStateSamplerAllocator( ob.ValidStateSamplerAllocator(allocMyValidStateSampler)) # create a planner for the defined space planner = og.PRM(si) ss.setPlanner(planner) # attempt to solve the problem within ten seconds of planning time solved = ss.solve(10.0) if solved: print("Found solution:") # print the path to screen print(ss.getSolutionPath()) else: print("No solution found")
def plan(): # construct the state space we are planning in space = ob.SE3StateSpace() # set the bounds for R^3 portion of SE(3) bounds = ob.RealVectorBounds(3) bounds.setLow(-10) bounds.setHigh(10) space.setBounds(bounds) # define a simple setup class ss = og.SimpleSetup(space) # create a start state start = ob.State(space) start().setX(-9) start().setY(-9) start().setZ(-9) start().rotation().setIdentity() # create a goal state goal = ob.State(space) goal().setX(-9) goal().setY(9) goal().setZ(-9) goal().rotation().setIdentity() ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) # set the start and goal states ss.setStartAndGoalStates(start, goal, 0.05) # Lets use PRM. It will have interesting PlannerData planner = og.PRM(ss.getSpaceInformation()) ss.setPlanner(planner) ss.setup() # attempt to solve the problem solved = ss.solve(20.0) if solved: # print the path to screen print("Found solution:\n%s" % ss.getSolutionPath()) # Extracting planner data from most recent solve attempt pd = ob.PlannerData(ss.getSpaceInformation()) ss.getPlannerData(pd) # Computing weights of all edges based on state space distance pd.computeEdgeWeights() if graphtool: useGraphTool(pd)
def newplanner(self, si): planner = og.PRM(si) return planner
start = ob.State(space) start()[0] = 0 start()[1] = 0 goal = ob.State(space) goal()[0] = 2 goal()[1] = 1.5 ss.setStartAndGoalStates(start, goal) si = ss.getSpaceInformation() si.setValidStateSamplerAllocator( ob.ValidStateSamplerAllocator(allocMyValidStateSampler)) planner = og.RRTstar(si) planner = og.PRM(si) ss.setPlanner(planner) solved = ss.solve(0.05) c.draw() if solved: ss.simplifySolution() pd = ob.PlannerData(ss.getSpaceInformation()) ss.getPlannerData(pd) pd.computeEdgeWeights() Vn = pd.numVertices() for n in range(0, Vn): v = pd.getVertex(n)
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.PRM(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