def test_optimal(calc_time=0.01): space = ob.RealVectorStateSpace(2) space.setBounds(0, WIDTH) si = ob.SpaceInformation(space) si.setStateValidityChecker(ValidityChecker(si)) si.setup() start = ob.State(space) goal = ob.State(space) start[0] = 0.0 start[1] = 0.0 goal[0] = WIDTH - 1 goal[1] = WIDTH - 1 pdef = ob.ProblemDefinition(si) pdef.setStartAndGoalStates(start, goal) objective = ob.MultiOptimizationObjective(si) objective.addObjective(MapCostObjective(si), 0.9) objective.addObjective(ob.PathLengthOptimizationObjective(si), 0.1) pdef.setOptimizationObjective(objective) planner = og.RRTstar(si) planner.setProblemDefinition(pdef) planner.setup() status = planner.solve(calc_time) if pdef.hasExactSolution(): states = pdef.getSolutionPath().getStates() cost = pdef.getSolutionPath().cost(objective).value() states_np = np.array([(state[0], state[1]) for state in states]) print(states) print(cost) print(states_np)
def __init__(self, lower_bound, upper_bound): """Initialize the object.""" # Create an R2 state space self.space = ob.RealVectorStateSpace(2) # Set lower and upper bounds bounds = ob.RealVectorBounds(2) bounds.setLow(lower_bound) bounds.setHigh(upper_bound) self.space.setBounds(bounds) # Create a new space information self.si = ob.SpaceInformation(self.space) # Set the state validity checker self.si.setStateValidityChecker( ob.StateValidityCheckerFn(self.is_state_valid)) self.si.setup() # Set the problem definition self.pdef = ob.ProblemDefinition(self.si) # Setup an optimizing planner with RRT* self.optimizingPlanner = og.RRTstar(self.si) # Empty obstacles list self.obstacles = []
def __init__(self, shape, calc_time, resolution, distance): ou.setLogLevel(ou.LOG_WARN) self._calc_time = calc_time self._space = ob.RealVectorStateSpace(2) bounds = ob.RealVectorBounds(2) bounds.setLow(0.0) bounds.setHigh(0, shape[0] - 1) bounds.setHigh(1, shape[1] - 1) self._space.setBounds(bounds) self._si = ob.SpaceInformation(self._space) self._validity_checker = OmplPlannerImpl.ValidityChecker(self._si) self._si.setStateValidityChecker(self._validity_checker) self._si.setStateValidityCheckingResolution(resolution) self._si.setup() self._start = ob.State(self._space) self._goal = ob.State(self._space) self._pdef = ob.ProblemDefinition(self._si) objective = ob.MultiOptimizationObjective(self._si) objective.addObjective(OmplPlannerImpl.MapCostObjective(self._si), 0.5) # ## setting length objective length_objective = ob.PathLengthOptimizationObjective(self._si) objective.addObjective(length_objective, 0.1) # objective.setCostThreshold(1000.0) self._pdef.setOptimizationObjective(objective) self._planner = og.RRTstar(self._si) self._planner.setRange(distance) self._planner.setup()
def setup_ompl(self): print "set space" self.space = ob.RealVectorStateSpace(dim=self.space_dimension) bounds = ob.RealVectorBounds(self.space_dimension) print "set bounds" for i in xrange(self.space_dimension): bounds.setLow(i, self.joint_constraints[0]) bounds.setHigh(i, self.joint_constraints[1]) print "set bounds 2" self.space.setBounds(bounds) print "set si" self.si = ob.SpaceInformation(self.space) print "set motion validator" self.motion_validator = MotionValidator(self.si) self.motion_validator.set_link_dimensions(self.link_dimensions) self.motion_validator.set_workspace_dimension(self.workspace_dimension) self.motion_validator.set_max_distance(self.max_velocity, self.delta_t) print "set state validiy checker" self.si.setStateValidityChecker( ob.StateValidityCheckerFn(self.motion_validator.isValid)) print "set motion validator" self.si.setMotionValidator(self.motion_validator) print "si.setup" self.si.setup() self.problem_definition = ob.ProblemDefinition(self.si)
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 execute(self, env, time, pathLength, show=False): result = True # instantiate space information si = mySpaceInformation(env) # instantiate problem definition pdef = ob.ProblemDefinition(si) # instantiate motion planner planner = self.newplanner(si) planner.setProblemDefinition(pdef) planner.setup() # the initial state state = ob.State(si) state()[0] = env.start[0] state()[1] = env.start[1] pdef.addStartState(state) goal = ob.GoalState(si) gstate = ob.State(si) gstate()[0] = env.goal[0] gstate()[1] = env.goal[1] goal.setState(gstate) goal.threshold = 1e-3 pdef.setGoal(goal) startTime = clock() if planner.solve(SOLUTION_TIME): elapsed = clock() - startTime time = time + elapsed if show: print('Found solution in %f seconds!' % elapsed) path = pdef.getSolutionPath() sm = og.PathSimplifier(si) startTime = clock() sm.reduceVertices(path) elapsed = clock() - startTime time = time + elapsed if show: print('Simplified solution in %f seconds!' % elapsed) path.interpolate(100) pathLength = pathLength + path.length() if show: print(env, '\n') temp = copy.deepcopy(env) for i in range(len(path.states)): x = int(path.states[i][0]) y = int(path.states[i][1]) if temp.grid[x][y] in [0, 2]: temp.grid[x][y] = 2 else: temp.grid[x][y] = 3 print(temp, '\n') else: result = False return (result, time, pathLength)
def _get_path( self, is_state_valid: Callable[[np.ndarray], bool], start_js: np.ndarray, robot_targ: RobotTarget, use_sim: MpSim, mp_space: MpSpace, timeout: int, ): """ Does the low-level path planning with OMPL. """ if not self._should_render: ou.setLogLevel(ou.LOG_ERROR) dim = mp_space.get_state_dim() space = ob.RealVectorStateSpace(dim) bounds = ob.RealVectorBounds(dim) lims = mp_space.get_state_lims() for i, lim in enumerate(lims): bounds.setLow(i, lim[0]) bounds.setHigh(i, lim[1]) space.setBounds(bounds) si = ob.SpaceInformation(space) si.setStateValidityChecker(ob.StateValidityCheckerFn(is_state_valid)) si.setup() pdef = ob.ProblemDefinition(si) mp_space.set_problem(pdef, space, si, start_js, robot_targ) planner = mp_space.get_planner(si) planner.setProblemDefinition(pdef) planner.setup() if mp_space.get_range() is not None: planner.setRange(mp_space.get_range()) solved = planner.solve(timeout) if not solved: self._log("Could not find plan") return None objective = pdef.getOptimizationObjective() if objective is not None: cost = (pdef.getSolutionPath().cost( pdef.getOptimizationObjective()).value()) else: cost = np.inf self._log("Got a path of length %.2f and cost %.2f" % (pdef.getSolutionPath().length(), cost)) path = pdef.getSolutionPath() joint_plan = mp_space.convert_sol(path) self._is_approx_sol = pdef.hasApproximateSolution() return joint_plan
def plan(runTime, plannerType, objectiveType, fname, bound, start_pt, goal_pt): print "Run Time: ", runTime # Construct the robot state space in which we're planning. We're # planning in [0,1]x[0,1], a subset of R^2. space = ob.RealVectorStateSpace(2) # Set the bounds of space to be in [0,1]. space.setBounds(bound[0], bound[1]) # 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 validityChecker = ValidityChecker(si) si.setStateValidityChecker(validityChecker) si.setup() # Set our robot's starting state to be the bottom-left corner of # the environment, or (0,0). start = ob.State(space) start[0] = start_pt[0] start[1] = start_pt[1] # Set our robot's goal state to be the top-right corner of the # environment, or (1,1). goal = ob.State(space) goal[0] = goal_pt[0] goal[1] = goal_pt[1] # 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)) # Construct the optimal planner specified by our command line argument. # This helper function is simply a switch statement. optimizingPlanner = allocatePlanner(si, plannerType) # Set the problem instance for our planner to solve optimizingPlanner.setProblemDefinition(pdef) optimizingPlanner.setup() # attempt to solve the planning problem in the given runtime solved = optimizingPlanner.solve(runTime) if solved: # Output the length of the path found print("{0} found solution of path length {1:.4f} with an optimization objective value of {2:.4f}".format(optimizingPlanner.getName(), pdef.getSolutionPath().length(), pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value())) # If a filename was specified, output the path as a matrix to # that file for visualization if fname: with open(fname,'w') as outFile: outFile.write(pdef.getSolutionPath().printAsMatrix()) return True, pdef.getSolutionPath().printAsMatrix() else: print("No solution found.") return False, "No solution found"
def problem_definition(self, space, space_info, start_pos, goal_pos): """ Function which define path problem that includes start and goal states between which the path must be built """ pdef = ob.ProblemDefinition(space_info) start = ob.State(space) start[0] = start_pos[0] start[1] = start_pos[1] goal = ob.State(space) goal[0] = goal_pos[0] goal[1] = goal_pos[1] pdef.setStartAndGoalStates(start, goal) return pdef
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 test_rigid(calc_time=0.01): space = ob.RealVectorStateSpace(2) space.setBounds(0, WIDTH) si = ob.SpaceInformation(space) si.setStateValidityChecker(ValidityChecker(si)) si.setup() start = ob.State(space) goal = ob.State(space) start[0] = 0.0 start[1] = 0.0 goal[0] = WIDTH - 1 goal[1] = WIDTH - 1 pdef = ob.ProblemDefinition(si) pdef.setStartAndGoalStates(start, goal) planner = og.RRTstar(si) planner.setProblemDefinition(pdef) planner.setup() status = planner.solve(calc_time) if pdef.hasExactSolution(): cost = pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value() print(cost)
def plan(self, start_row, start_col, goal_row, goal_col, plannerType, runtime): if not self.si: return False start = ob.State(self.space) start()[0] = start_row start()[1] = start_col goal = ob.State(self.space) goal()[0] = goal_row goal()[1] = goal_col # Create a problem instance self.pdef = ob.ProblemDefinition(self.si) # Set the start and goal states self.pdef.setStartAndGoalStates(start, goal) self.planner = self.allocatePlanner(self.si, plannerType) self.planner.setProblemDefinition(self.pdef) # perform setup steps for the planner self.planner.setup() # print the settings for this space print(self.si.settings()) # generate a few solutions; all will be added to the goal for i in range(5): self.solved = self.planner.solve(runtime) p = self.pdef.getSolutionPath() print("Found solution:\n%s" % p) ns = self.planner.getProblemDefinition().getSolutionCount() print("Found %d solutions" % ns) if self.solved: # get the goal representation from the problem definition (not the same as the goal state) # and inquire about the found path path = self.pdef.getSolutionPath() print("Found solution:\n%s" % path) return True else: print("No solution found") return False
def plan(runTime, plannerType, objectiveType, fname): # Construct the robot state space in which we're planning. We're # planning in [0,500]x[0,500], a subset of R^2. space = ob.RealVectorStateSpace(2) # Set the bounds of space to be in [0,500]. space.setBounds(0.0, 500.0) # 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 validityChecker = ValidityChecker(si) si.setStateValidityChecker(validityChecker) si.setup() # Set our robot's starting state to be random start = ob.State(space) start[0] = random.randint(0,500) start[1] = random.randint(0,500) while (sqrt((start[0]-center[0])*(start[0]-center[0]) + (start[1]-center[0])*(start[1]-center[0])) - radius < 0): start[0] = random.randint(0,500) start[1] = random.randint(0,500) # Set our robot's goal state to be random goal = ob.State(space) goal[0] = random.randint(0,500) goal[1] = random.randint(0,500) while (sqrt((goal[0]-center[0])*(goal[0]-center[0]) + (goal[1]-center[0])*(goal[1]-center[0])) - radius < 0): goal[0] = random.randint(0,500) goal[1] = random.randint(0,500) # 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)) # Construct the optimal planner specified by our command line argument. # This helper function is simply a switch statement. optimizingPlanner = allocatePlanner(si, plannerType) # Set the problem instance for our planner to solve optimizingPlanner.setProblemDefinition(pdef) optimizingPlanner.setup() # attempt to solve the planning problem in the given runtime solved = optimizingPlanner.solve(runTime) if solved: # Output the length of the path found print('{0} found solution of path length {1:.4f} with an optimization ' \ 'objective value of {2:.4f}'.format( \ optimizingPlanner.getName(), \ pdef.getSolutionPath().length(), \ pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value())) # If a filename was specified, output the path as a matrix to # that file for visualization if fname: with open(fname, 'w') as outFile: outFile.write(pdef.getSolutionPath().printAsMatrix()) else: print("No solution found.")
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.RRTXstatic(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
def plan(self, runTime, plannerType, objectiveType, fname): # Construct the robot state space in which we're planning. We're # planning in [0,1]x[0,1], a subset of R^2. self.space = ob.RealVectorStateSpace(2) # Set the bounds of space to be in [0,1]. self.space.setBounds(0, 400.0) # Construct a space information instance for this state space self.si = ob.SpaceInformation(self.space) # Set the object used to check which states in the space are valid validityChecker = ValidityChecker(self.si, self.mapIm) self.si.setStateValidityChecker(validityChecker) self.si.setup() self.start = ob.State(self.space) self.start[0] = self.startPose[0] self.start[1] = self.startPose[1] self.goal = ob.State(self.space) self.goal[0] = self.goalPose[0] self.goal[1] = self.goalPose[1] # Create a problem instance self.pdef = ob.ProblemDefinition(self.si) # Set the start and goal states self.pdef.setStartAndGoalStates(self.start, self.goal) # Create the optimization objective specified by our command-line argument. # This helper function is simply a switch statement. self.pdef.setOptimizationObjective( self.allocateObjective(self.si, objectiveType)) # Construct the optimal planner specified by our command line argument. # This helper function is simply a switch statement. self.optimizingPlanner = self.allocatePlanner(self.si, plannerType) # Set the problem instance for our planner to solve self.optimizingPlanner.setProblemDefinition(self.pdef) self.optimizingPlanner.setup() # attempt to solve the planning problem in the given runtime solved = self.optimizingPlanner.solve(runTime) if solved: # Output the length of the path found print( "{0} found solution of path length {1:.4f} with an optimization objective value of {2:.4f}" .format( self.optimizingPlanner.getName(), self.pdef.getSolutionPath().length(), self.pdef.getSolutionPath().cost( self.pdef.getOptimizationObjective()).value())) print self.pdef.getSolutionPath() # If a filename was specified, output the path as a matrix to # that file for visualization # if fname: # with open(fname,'w') as outFile: # outFile.write(pdef.getSolutionPath().printAsMatrix()) else: print("No solution found.")
def plan(self, controlled_object, x1, y1, v1, a1): a1 = (a1 + 2 * np.pi) % (2 * np.pi) car = deepcopy(controlled_object) space = ob.RealVectorStateSpace(3) bounds = ob.RealVectorBounds(3) bounds.setLow(0, 0) bounds.setLow(1, 0) bounds.setLow(2, 0) bounds.setHigh(0, 1000) bounds.setHigh(1, 1000) bounds.setHigh(2, 2 * np.pi) space.setBounds(bounds) si = ob.SpaceInformation(space) validityChecker = ValidityChecker(si, self.state, car) si.setStateValidityChecker(validityChecker) #si.setMotionValidator(MotionValidityChecker(si, state, agent_num)) si.setup() start = ob.State(space) start[0] = car.x start[1] = car.y start[2] = car.angle goal = ob.State(space) goal[0] = x1 goal[1] = y1 goal[2] = a1 pdef = ob.ProblemDefinition(si) pdef.setStartAndGoalStates(start, goal) pdef.setOptimizationObjective(ob.PathLengthOptimizationObjective(si)) def objStateCost(state): return ob.Cost(0) def objMotionCost(state1, state2): x, y, a = state1[0], state1[1], state1[2] xt, yt, at = state2[0], state2[1], state2[2] yd, xd = yt - y, xt - x ax = np.arctan(yd / (xd + 0.001)) ax = ax % (np.pi) if yd < 0: ax = ax + np.pi ad = min(np.abs(at - ax), np.abs((ax - at))) return 10 * ad pathObj = ob.OptimizationObjective(si) pathObj.stateCost = objStateCost pathObj.motionCost = objMotionCost pathObj.setCostThreshold(1) #pdef.setOptimizationObjective(pathObj) optimizingPlanner = og.RRTstar(si) optimizingPlanner.setRange(self.inter_point_d) optimizingPlanner.setProblemDefinition(pdef) optimizingPlanner.setup() solved = optimizingPlanner.solve(self.planning_time) sol = pdef.getSolutionPath() if not sol: return [] sol = sol.printAsMatrix() s = [[float(j) for j in i.split(" ")[:-1]] for i in sol.splitlines()][:-1] v0 = car.vel num_points = len(s) for i in range(num_points): [i].append(v0 * (1 - float(i) / float(num_points)) + v1 * (float(i) / float(num_points))) return s
si_R2.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid_R2)) # Create vector of spaceinformationptr si_vec = og.vectorSpaceInformation() si_vec.append(si_R2) si_vec.append(si_SE2) # Define Planning Problem start_SE2 = ob.State(SE2) goal_SE2 = ob.State(SE2) start_SE2().setXY(0, 0) start_SE2().setYaw(0) goal_SE2().setXY(1, 1) goal_SE2().setYaw(0) pdef = ob.ProblemDefinition(si_SE2) pdef.setStartAndGoalStates(start_SE2, goal_SE2) # Setup Planner using vector of spaceinformationptr planner = og.QRRT(si_vec) # Planner can be used as any other OMPL algorithm planner.setProblemDefinition(pdef) planner.setup() solved = planner.solve(1.0) if solved: print('-' * 80) print('Configuration-Space Path (SE2):') print('-' * 80)
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
def plan_trajectory(self, start_point, goal_point, planner_number, joint_names, group_name, planning_time, planner_config_name, plan_type='pfs'): """ Use OMPL library for planning. Obtain obstacle information from rostopic for collision checking """ # obtain obstacle information through rostopic rospy.loginfo( "%s Plan Trajectory Wrapper: waiting for obstacle message..." % (rospy.get_name())) obc = rospy.wait_for_message('obstacles/obc', Float64Array2D) # obs = rospy.wait_for_message('obstacles/obs', Float64Array2D) obc = [obc_i.values for obc_i in obc.points] obc = np.array(obc) rospy.loginfo( "%s Plan Trajectory Wrapper: obstacle message received." % (rospy.get_name())) # depending on plan type, obtain path_length from published topic or not if plan_type == 'pfs': # obtain path length through rostopic rospy.loginfo( "%s Plan Trajectory Wrapper: waiting for planning path length message..." % (rospy.get_name())) path_length = rospy.wait_for_message( 'planning/path_length_threshold', Float64) path_length = path_length.data rospy.loginfo( "%s Plan Trajectory Wrapper: planning path length received." % (rospy.get_name())) elif plan_type == 'rr': path_length = np.inf # set a very large path length because we only want feasible paths # reshape # plan IsInCollision = self.IsInCollision rospy.loginfo("%s Plan Trajectory Wrapper: start planning..." % (rospy.get_name())) # create a simple setup object start = ob.State(self.space) # we can pick a random start state... # ... or set specific values for k in xrange(len(start_point)): start[k] = start_point[k] goal = ob.State(self.space) for k in xrange(len(goal_point)): goal[k] = goal_point[k] def isStateValid(state): return not IsInCollision(state, obc) si = ob.SpaceInformation(self.space) si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) si.setup() pdef = ob.ProblemDefinition(si) pdef.setStartAndGoalStates(start, goal) pdef.setOptimizationObjective(getPathLengthObjective(si, path_length)) ss = allocatePlanner(si, self.planner_name) ss.setProblemDefinition(pdef) ss.setup() plan_time = time.time() solved = ss.solve(planning_time) plan_time = time.time() - plan_time if solved: rospy.loginfo( "%s Plan Trajectory Wrapper: OMPL Planner solved successfully." % (rospy.get_name())) # obtain planned path ompl_path = pdef.getSolutionPath().getStates() solutions = np.zeros((len(ompl_path), 2)) for k in xrange(len(ompl_path)): solutions[k][0] = float(ompl_path[k][0]) solutions[k][1] = float(ompl_path[k][1]) return plan_time, solutions.tolist() else: return np.inf, None
def plan(self, runTime, plannerType, objectiveType, fname): # Construct the robot state space in which we're planning. We're # planning in [0,1]x[0,1], a subset of R^2. space = ob.RealVectorStateSpace(2) # Set the bounds of space to be in [0,1]. space.setBounds(0.0, 1.0) # 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 validityChecker = ValidityChecker(si) si.setStateValidityChecker(validityChecker) si.setup() # Set our robot's starting state to be the bottom-left corner of # the environment, or (0,0). start = ob.State(space) start[0] = self.start_x start[1] = self.start_y # Set our robot's goal state to be the top-right corner of the # environment, or (1,1). goal = ob.State(space) goal[0] = self.goal_x goal[1] = self.goal_y print("goal") print self.goal_x print self.goal_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)) # Construct the optimal planner specified by our command line argument. # This helper function is simply a switch statement. optimizingPlanner = allocatePlanner(si, plannerType) # Set the problem instance for our planner to solve optimizingPlanner.setProblemDefinition(pdef) optimizingPlanner.setup() # attempt to solve the planning problem in the given runtime solved = optimizingPlanner.solve(runTime) if solved: # Output the length of the path found #print("{0} found solution of path length {1:.4f} with an optimization objective value of {2:.4f}".format(optimizingPlanner.getName(), pdef.getSolutionPath().length(), pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value())) str = pdef.getSolutionPath().printAsMatrix() #print(type(str)) # x = str.split() self.path = [float(x) for x in x if x] #print(self.path) #talker() else: print("No solution found.") return self.path
def plan(fname=None): # Construct the robot state space in which we're planning. We're # planning in [0,1]x[0,1], a subset of R^2. space = ob.RealVectorStateSpace(2) # Set the bounds of space to be in [0,1]. space.setBounds(0.0, 1.0) # 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 validityChecker = ValidityChecker(si) si.setStateValidityChecker(validityChecker) si.setup() # Set our robot's starting state to be the bottom-left corner of # the environment, or (0,0). start = ob.State(space) start[0] = 0.0 start[1] = 0.0 # Set our robot's goal state to be the top-right corner of the # environment, or (1,1). goal = ob.State(space) goal[0] = 1.0 goal[1] = 1.0 # Create a problem instance pdef = ob.ProblemDefinition(si) # Set the start and goal states pdef.setStartAndGoalStates(start, goal) # Since we want to find an optimal plan, we need to define what # is optimal with an OptimizationObjective structure. Un-comment # exactly one of the following 6 lines to see some examples of # optimization objectives. pdef.setOptimizationObjective(getPathLengthObjective(si)) # pdef.setOptimizationObjective(getThresholdPathLengthObj(si)) # pdef.setOptimizationObjective(getClearanceObjective(si)) # pdef.setOptimizationObjective(getBalancedObjective1(si)) # pdef.setOptimizationObjective(getBalancedObjective2(si)) # pdef.setOptimizationObjective(getPathLengthObjWithCostToGo(si)) # Construct our optimal planner using the RRTstar algorithm. optimizingPlanner = og.RRTstar(si) # Set the problem instance for our planner to solve optimizingPlanner.setProblemDefinition(pdef) optimizingPlanner.setup() # attempt to solve the planning problem within one second of # planning time solved = optimizingPlanner.solve(10.0) if solved: # Output the length of the path found print("Found solution of path length %g" % pdef.getSolutionPath().length()) # If a filename was specified, output the path as a matrix to # that file for visualization if fname: with open(fname, 'w') as outFile: outFile.write(pdef.getSolutionPath().printAsMatrix()) else: print("No solution found.")
def plan(mapfile,start_node, goal_node,runTime, plannerType, objectiveType, d, fname): boundary, blocks = load_map(mapfile) boundary[:,:3] = boundary[:,:3] +0.0005 boundary[:,3:] = boundary[:,3:] -0.0005 alpha = 1.05 blocks = blocks*alpha # Construct the robot state space in which we're planning. We're space = ob.RealVectorStateSpace(3) sbounds = ob.RealVectorBounds(3) sbounds.low[0] =float(boundary[0,0]) sbounds.high[0] =float(boundary[0,3]) sbounds.low[1] = float(boundary[0,1]) sbounds.high[1] = float(boundary[0,4]) sbounds.low[2] = float(boundary[0,2]) sbounds.high[2] = float(boundary[0,5]) space.setBounds(sbounds) # 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 validityChecker = ValidityChecker(si, boundary, blocks) si.setStateValidityChecker(validityChecker) mv = MyMotionValidator(si, boundary, blocks) si.setMotionValidator(mv) si.setup() # Set our robot's starting state start = ob.State(space) start[0] = start_node[0] start[1] = start_node[1] start[2] = start_node[2] # Set our robot's goal state goal = ob.State(space) goal[0] = goal_node[0] goal[1] = goal_node[1] goal[2] = goal_node[2] # 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)) # Construct the optimal planner specified by our command line argument. # This helper function is simply a switch statement. optimizingPlanner = allocatePlanner(si,d, plannerType) # Set the problem instance for our planner to solve optimizingPlanner.setProblemDefinition(pdef) optimizingPlanner.setup() solved = "Approximate solution" # attempt to solve the planning problem in the given runtime t1 = tic() while(str(solved) == 'Approximate solution'): solved = optimizingPlanner.solve(runTime) # print(pdef.getSolutionPath().printAsMatrix()) t2 = toc(t1) p = pdef.getSolutionPath() ps = og.PathSimplifier(si) ps.simplifyMax(p) if solved: # Output the length of the path found print('{0} found solution of path length {1:.4f} with an optimization ' \ 'objective value of {2:.4f}'.format( \ optimizingPlanner.getName(), \ pdef.getSolutionPath().length(), \ pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value())) # If a filename was specified, output the path as a matrix to # that file for visualization env = mapfile.split(".")[1].split("/")[-1] fname = fname + "{}_{}_{}_{}.txt".format(env, d,np.round(pdef.getSolutionPath().length(),2),np.round(t2,2)) if fname: with open(fname, 'w') as outFile: outFile.write(pdef.getSolutionPath().printAsMatrix()) path = np.genfromtxt(fname) print("The found path : ") print(path) pathlength = np.sum(np.sqrt(np.sum(np.diff(path,axis=0)**2,axis=1))) print("pathlength : {}".format(pathlength)) fig, ax, hb, hs, hg = draw_map(boundary, blocks, start_node, goal_node) ax.plot(path[:,0],path[:,1],path[:,2],'r-') plt.show()
def plan(runTime, plannerType, objectiveType, fname): # Construct the robot state space in which we're planning. We're # planning in [0,1]x[0,1], a subset of R^2. space = ob.SE2StateSpace() # Set the bounds of space to be in [0,1] bounds = ob.RealVectorBounds(2) bounds.setLow(0) bounds.setHigh(1) space.setBounds(bounds) # 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 validityChecker = ValidityChecker(si) si.setStateValidityChecker(validityChecker) si.setup() # Set our robot's starting state to be the bottom-left corner of # the environment, or (0,0). start = ob.State(space) start[0] = 0 start[1] = 0 # Set our robot's goal state to be the top-right corner of the # environment, or (1,1). goal = ob.State(space) goal[0] = 1 goal[1] = 1 # 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)) # Construct the optimal planner specified by us. # This helper function is simply a switch statement. optimizingPlanner = allocatePlanner(si, plannerType) # Set the problem we are trying to solve to the planner optimizingPlanner.setProblemDefinition(pdef) #perform setup steps for the planner optimizingPlanner.setup() #print the settings for this space #print(si.settings()) #print the problem settings #print(pdef) # attempt to solve the planning problem in the given runtime solved = optimizingPlanner.solve(runTime) if solved: #print solution path path = pdef.getSolutionPath() print("Found Solution:\n%s" % path) # Output the length of the path found print('{0} found solution of path length {1:.4f} with an optimization ' \ 'objective value of {2:.4f}'.format( \ optimizingPlanner.getName(), \ pdef.getSolutionPath().length(), \ pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value())) # If a filename was specified, output the path as a matrix to # that file for visualization if fname: with open(fname, 'w') as outFile: outFile.write(pdef.getSolutionPath().printAsMatrix()) print("saved final path as 'mat.txt' for matplotlib") # Extracting planner data from most recent solve attempt pd = ob.PlannerData(pdef.getSpaceInformation()) optimizingPlanner.getPlannerData(pd) # Computing weights of all edges based on state space distance pd.computeEdgeWeights() #dataStorage=ob.PlannerDataStorage() #dataStorage.store(pd,"myPlannerData") graphml = pd.printGraphML() f = open("graph.graphml", 'w') f.write(graphml) f.close() print("saved") else: print("No solution found.")
def OMPL_plan(self, runTime, plannerType, objectiveType, mult): # Construct the robot state space in which we're planning. We're # planning in [0,1]x[0,1], a subset of R^2. if self.dimension == '2D': space = ob.RealVectorStateSpace(2) # Set the bounds of space to be in [0,1]. bounds = ob.RealVectorBounds(2) x_bound = [] y_bound = [] for idx in range(len(self.region)): x_bound.append(self.region[idx][0]) y_bound.append(self.region[idx][1]) bounds.setLow(0, min(x_bound)) bounds.setHigh(0, max(x_bound)) bounds.setLow(1, min(y_bound)) bounds.setHigh(1, max(y_bound)) self.x_bound = (min(x_bound), max(x_bound)) self.y_bound = (min(y_bound), max(y_bound)) #test = bounds.getDifference() #test = bounds.check() space.setBounds(bounds) # Set our robot's starting state to be the bottom-left corner of # the environment, or (0,0). start = ob.State(space) start[0] = self.start[0] start[1] = self.start[1] #start[2] = 0.0 # Set our robot's goal state to be the top-right corner of the # environment, or (1,1). goal = ob.State(space) goal[0] = self.goal[0] goal[1] = self.goal[1] #goal[2] = 1.0 else: pass # 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 validityChecker = self.ValidityChecker(si) validityChecker.setInteractive() validityChecker.obstacle(self.start, self.goal, self.region, self.obstacle, self.dimension) si.setStateValidityChecker(validityChecker) si.setup() # 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(self.allocateObjective( si, objectiveType)) # Construct the optimal planner specified by our command line argument. # This helper function is simply a switch statement. optimizingPlanner = self.allocatePlanner(si, plannerType) # Set the problem instance for our planner to solve optimizingPlanner.setProblemDefinition(pdef) optimizingPlanner.setRewireFactor(1.1) optimizingPlanner.setup() # attempt to solve the planning problem in the given runtime solved = optimizingPlanner.solve(runTime) self.PlannerStates.append( (validityChecker.getInteractive(), plannerType)) if solved: # Output the length of the path found try: print('{0} found solution of path length {1:.4f} with an optimization ' \ 'objective value of {2:.4f}'.format( \ optimizingPlanner.getName(), \ pdef.getSolutionPath().length(), \ pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value())) return self.decodeSolutionPath( pdef.getSolutionPath().printAsMatrix(), plannerType, pdef.getSolutionPath().cost( pdef.getOptimizationObjective()).value(), mult) except: print("No solution found.") pass else: print("No solution found.")