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 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 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.FMT(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(samplerIndex, start, goal, ss, space, display_trajectory_publisher): si = ss.getSpaceInformation() if samplerIndex == 1: # use obstacle-based sampling space.setStateSamplerAllocator( ob.StateSamplerAllocator(allocSelfCollisionFreeStateSampler)) ss.setStartAndGoalStates(start, goal) # create a planner for the defined space planner = og.FMT(si) # change this to FMT; if samplerIndex == 1: planner.setVAEFMT( 1) # This flag is for turning on sampling with the VAE generator; # set parameter; planner.setExtendedFMT( False) # do not extend if the planner does not terminate; planner.setNumSamples(100) planner.setNearestK(False) planner.setCacheCC(True) planner.setHeuristics(True) # planner.setNearestK(1) # Disable K nearest neighbor implementation; ss.setPlanner(planner) start_time = time.time() solved = ss.solve(40.0) elapsed_time = time.time() - start_time if solved: print("Found solution after %s seconds:" % elapsed_time) # print the path to screen path = ss.getSolutionPath() # ("The solution is: %s" % path) # Visualization display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = convertStateToRobotState(start) trajectory = convertPlanToTrajectory(path) display_trajectory.trajectory.append(trajectory) display_trajectory_publisher.publish(display_trajectory) print("Visualizing trajectory...") sleep(0.5) else: print("No solution found after %s seconds: " % elapsed_time) return elapsed_time, float((int(solved)))
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.FMT(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) else: print("No solution found") #metrics generation and graphing is possible here # #return trace for _find_path_internal method print( "$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$" ) 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