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 __init__(self): img = Image.open('/home/alphonsus/ompl_code/test_codes/rrt_2d/rrt.png') self.env = np.array(img, dtype='uint8') self.maxWidth = self.env.shape[0] - 1 self.maxHeight = self.env.shape[1] - 1 # bounds = ob.RealVectorBounds(2) # bounds.setLow(0,0); bounds.setHigh(0,self.env.shape[0]) # bounds.setLow(1,0); bounds.setHigh(1,self.env.shape[1]) # self.space = ob.SE2StateSpace() self.space = ob.RealVectorStateSpace() self.space.addDimension(0.0, self.env.shape[0]) self.space.addDimension(0.0, self.env.shape[1]) # self.space.setBounds(bounds) self.ss_ = og.SimpleSetup(self.space) self.ss_.setStateValidityChecker( ob.StateValidityCheckerFn(self.isStateValid)) self.space.setup() self.ss_.getSpaceInformation().setStateValidityCheckingResolution( 1.0 / self.space.getMaximumExtent()) self.ss_.setPlanner(og.RRTConnect(self.ss_.getSpaceInformation())) self.ss_.setup()
def plan_path(self): self.problem_definition.clearSolutionPaths() path_collides = True if self.use_linear_path: path = self.linear_path(self.start_state, self.goal_state) path_collides = self.path_collides(path) if path_collides: if not self.motion_validator.isValid( self.start_state) or not self.motion_validator.isValid( self.goal_state): logging.warn( "PathPlanner: Start or goal state not valid. Skipping") return [], [], [] self.problem_definition.addStartState(self.start_state) self.problem_definition.setStartAndGoalStates( self.start_state, self.goal_state) self.planner = og.RRTConnect(self.si) self.planner.setRange( np.sqrt(self.si.getStateSpace().getDimension() * np.square(self.delta_t * self.max_velocity))) self.planner.setProblemDefinition(self.problem_definition) self.planner.setup() start_time = time.time() while not self.problem_definition.hasSolution(): self.planner.solve(1.0) delta = time.time() - start_time logging.info("PathPlanner: Time spent on planning so far: " + str(delta)) if delta > 3.0: xs = [] us = [] zs = [] return xs, us, zs path = [] if self.problem_definition.hasSolution(): logging.info("PathPlanner: Solution found after " + str(time.time() - start_time) + " seconds") solution_path = self.problem_definition.getSolutionPath() states = solution_path.getStates() path = [ np.array( [state[i] for i in xrange(self.space.getDimension())]) for state in states ] #print "path " + str(path) else: print "no solution" return self._augment_path(path)
def plan(): # create an Vector state space space = ob.RealVectorStateSpace(SPACE_DIM) # print(space) # # set lower and upper bounds bounds = ob.RealVectorBounds(SPACE_DIM) for i in range(SPACE_DIM): bounds.setLow(i, -1) bounds.setHigh(i, 1) space.setBounds(bounds) # create a simple setup object ss = og.SimpleSetup(space) # set planner planner = og.RRTConnect(ss.getSpaceInformation()) ss.setPlanner(planner) ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) start = ob.State(space) # we can pick a random start state... start.random() start_state = start.get() for i in range(SPACE_DIM): # start_state[i] = 0.1 print(start_state[i]) goal = ob.State(space) # we can pick a random goal state... goal.random() ss.setStartAndGoalStates(start, goal) # default parameters solved = ss.solve(1.0) if solved: # try to shorten the path ss.simplifySolution() # print the simplified path print(ss.getSolutionPath())
def plan_path(self): self.problem_definition.clearSolutionPaths() path_collides = True if (not self.motion_validator.isValid(self.start_state) or not self.problem_definition.getGoal().canSample()): return [], [], [] if self.use_linear_path: path = self.linear_path( self.start_state, self.problem_definition.getGoal().getRandomGoalState()) path_collides = self.path_collides(path) if path_collides: self.problem_definition.addStartState(self.start_state) self.planner = og.RRTConnect(self.si) self.planner.setRange( np.sqrt(self.si.getStateSpace().getDimension() * np.square(self.delta_t * self.max_velocity))) self.planner.setProblemDefinition(self.problem_definition) self.planner.setup() start_time = time.time() while not self.problem_definition.hasSolution(): self.planner.solve(1.0) delta = time.time() - start_time if delta > 3.0: #logging.warn("PathPlanner: Maximum allowed planning time exceeded. Aborting") xs = [] us = [] zs = [] return xs, us, zs path = [] if self.problem_definition.hasSolution(): logging.info("PathPlanner: Solution found after " + str(time.time() - start_time) + " s") solution_path = self.problem_definition.getSolutionPath() states = solution_path.getStates() path = [ np.array( [state[i] for i in xrange(self.space.getDimension())]) for state in states ] return self._augment_path(path)
def planTheHardWay(): # create an SE2 state space space = ob.SE2StateSpace() # set lower and upper bounds bounds = ob.RealVectorBounds(2) bounds.setLow(-1) bounds.setHigh(1) 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 a random start state start = ob.State(space) start.random() # create a random goal state goal = ob.State(space) goal.random() # 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.RRTConnect(si) # set the problem we are trying to solve for the planner planner.setProblemDefinition(pdef) # perform setup steps for the planner planner.setup() # print the settings for this space print(si.settings()) # print the problem settings print(pdef) # 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%s" % path) else: print("No solution found")
def plan(): # create an SE2 state space space = ob.SE2StateSpace() # set lower and upper bounds bounds = ob.RealVectorBounds(2) bounds.setLow(-1) bounds.setHigh(1) space.setBounds(bounds) # create a simple setup object ss = og.SimpleSetup(space) # set planner planner = og.RRTConnect(ss.getSpaceInformation()) ss.setPlanner(planner) ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) start = ob.State(space) # we can pick a random start state... start.random() # ... or set specific values start().setX(.5) goal = ob.State(space) # we can pick a random goal state... goal.random() # ... or set specific values goal().setX(-.5) ss.setStartAndGoalStates(start, goal) # default parameters solved = ss.solve(1.0) if solved: # try to shorten the path ss.simplifySolution() # print the simplified path print(ss.getSolutionPath())
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.")
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 newplanner(self, si): planner = og.RRTConnect(si) planner.setRange(10.0) return planner
goal1 = goal()[0] goal1.setX(200.49) goal1.setY(-40.62) goal1.setZ(70.57) goal1.rotation().setIdentity() # set goal for robot 2 goal2 = goal()[1] goal2.setX(-4.96) goal2.setY(-40.62) goal2.setZ(70.57) goal2.rotation().setIdentity() # set the start & goal states setup.setStartAndGoalStates(start, goal) # setting collision checking resolution to 1% of the space extent setup.getSpaceInformation().setStateValidityCheckingResolution(0.01) # use RRTConnect for planning setup.setPlanner(og.RRTConnect(setup.getSpaceInformation())) # we call setup just so print() can show more information setup.setup() print(setup) # try to solve the problem if setup.solve(60): # simplify & print the solution setup.simplifySolution() print(setup.getSolutionPath().printAsMatrix())
def get_planner(self, si): return og.RRTConnect(si)
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.RRTConnect(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")