def planning(self, runtime=50.0): if self.start is None and self.goal is None: return False self.ss.clear() dt = 5.0 tic = time.time() find_exact_solution = False self.ss.setStartAndGoalStates(self.start, self.goal, 1.0) for _ in np.arange(0.0, runtime+dt, dt): self.ss.solve(dt) path_matrix = self.ss.getSolutionPath().printAsMatrix() path = np.array([j.split() for j in path_matrix.splitlines()], dtype=float) if self.check_reach(path): find_exact_solution = True break toc = time.time() self.planning_time = toc - tic self.path = path # get planner data pd = ob.PlannerData(self.ss.getSpaceInformation()) self.ss.getPlannerData(pd) pd.computeEdgeWeights() xmlstring = pd.printGraphML() self.planner_data = self.xml2tree(xmlstring) self.reach_exactly = find_exact_solution return find_exact_solution
def plan(): # construct the state space we are planning in space = ob.SE2StateSpace() # set the bounds for R^3 portion of SE(3) bounds = ob.RealVectorBounds(2) bounds.setLow(0, -40) bounds.setHigh(0, 40) bounds.setLow(1, 0) bounds.setHigh(1, 90) space.setBounds(bounds) # define a simple setup class ss = og.SimpleSetup(space) # create a start state start = ob.State(space) start().setX(0) start().setY(0) start().setYaw(0) # start().setZ(-9) # start().rotation().setIdentity() # create a goal state goal = ob.State(space) goal().setX(-25) goal().setY(60) goal().setYaw(0) # 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.RRTstar(ss.getSpaceInformation()) ss.setPlanner(planner) ss.setup() # attempt to solve the problem # To change time change value given to ss.solve solved = ss.solve(1.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() path = get_path(ss) draw_graph(path) plt.show()
def plan(runTime, plannerType, objectiveType, plotData, fname, pdfname): space = ob.RealVectorStateSpace(2) space.setBounds(0.0, 1.0) ss = og.SimpleSetup(space) start = ob.State(space) start[0] = 0.0 start[1] = 0.0 goal = ob.State(space) goal[0] = 1.0 goal[1] = 1.0 ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) ss.setStartAndGoalStates(start, goal) si = ss.getSpaceInformation() # Create the optimization objective specified by our command-line argument. # This helper function is simply a switch statement. ss.setOptimizationObjective(allocateObjective(si, objectiveType)) ss.setPlanner(allocatePlanner(si, plannerType)) ss.setup() # attempt to solve the problem solved = ss.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( \ ss.getSolutionPlannerName(), \ ss.getSolutionPath().length(), \ ss.getSolutionPath().cost(ss.getOptimizationObjective()).value())) # print the path to screen print("Found solution:\n%s" % ss.getSolutionPath()) # Extracting planner data from most recent solve attempt pd = ob.PlannerData(si) ss.getPlannerData(pd) # Computing weights of all edges based on state space distance pd.computeEdgeWeights() if graphtool and plotData: useGraphTool(pd) # 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(ss.getSolutionPath().printAsMatrix()) if pdfname: pds = ob.PlannerDataStorage() pds.store(pd, pdfname) else: print("No solution found.")
def plan(): # create an Vector state space space = ob.RealVectorStateSpace(SPACE_DIM) # # set lower and upper bounds bounds = ob.RealVectorBounds(SPACE_DIM) for i in range(SPACE_DIM - 3): bounds.setLow(i, min_bound) bounds.setHigh(i, max_bound) for i in range(SPACE_DIM - 3): bounds.setLow(i + 3, -np.pi) bounds.setHigh(i + 3, np.pi) space.setBounds(bounds) space.setBounds(bounds) # create a simple setup object ss = og.SimpleSetup(space) # set planner planner = og.RRTstar(ss.getSpaceInformation()) ss.setPlanner(planner) ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) start = ob.State(space) # start.random() start_state = start.get() for i in range(SPACE_DIM): start_state[i] = start_s[i] # print(start_state[i]) goal = ob.State(space) # goal.random() goal_state = goal.get() for i in range(SPACE_DIM): goal_state[i] = goal_s[i] # print(goal_state[i]) ss.setStartAndGoalStates(start, goal) # default parameters solved = ss.solve(4.0) if solved: # パスの簡単化を実施 ss.simplifySolution() # 結果を取得 path_result = ss.getSolutionPath() print(path_result) si = ss.getSpaceInformation() pdata = ob.PlannerData(si) ss.getPlannerData(pdata) space = path_result.getSpaceInformation().getStateSpace() plot_result(space, path_result, pdata)
def dumpGraph(self, name): ou.OMPL_INFORM("Dumping planner graph to `%s_graph.graphml`." % name) data = ob.PlannerData(self.csi) self.pp.getPlannerData(data) with open("%s_graph.graphml" % name, "w") as graphfile: print(data.printGraphML(), file=graphfile) if self.spaceType == "AT" or self.spaceType == "TB": ou.OMPL_INFORM("Dumping atlas to `%s_atlas.ply`." % name) with open("%s_atlas.ply" % name, "w") as atlasfile: print(self.css.printPLY(), file=atlasfile)
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 plt_ompl_result(condition, start, goal, path, collisionchecker, spaceinfo, planner): fig1 = plt.figure(figsize=(10, 6), dpi=80) ax1 = fig1.add_subplot(111, aspect='equal') obs, dimW = gap2obs(condition) for i in range(0, obs.shape[0] // (2 * dimW)): # plot obstacle patches ax1.add_patch( patches.Rectangle( (obs[i * 2 * dimW], obs[i * 2 * dimW + 1]), # (x,y) obs[i * 2 * dimW + dimW] - obs[i * 2 * dimW], # width obs[i * 2 * dimW + dimW + 1] - obs[i * 2 * dimW + 1], # height alpha=0.6 )) gridSize = 11 plannerdata = ob.PlannerData(spaceinfo) planner.getPlannerData(plannerdata) print("num edges: ", plannerdata.numEdges()) print("num vertices: ", plannerdata.numVertices()) num_ver = plannerdata.numVertices() num_edge = plannerdata.numEdges() if num_edge > 3000: print("too much edges") edge_vis = False else: edge_vis = True for i in range(0, num_ver): plt.scatter( plannerdata.getVertex(i).getState()[0], plannerdata.getVertex(i).getState()[1], color="green", s=50, edgecolors='black') # vertice if edge_vis: for j in range(0, num_ver): if plannerdata.edgeExists(i, j): plt.plot([plannerdata.getVertex(i).getState()[0], plannerdata.getVertex(j).getState()[0]], [plannerdata.getVertex(i).getState()[1], plannerdata.getVertex(j).getState()[1]], color='gray') path.interpolate() states = path.getStates() for state in states: plt.scatter(state[0], state[1], color="green", s=250, edgecolors='black') # path # for state in collisionchecker.states_ok: # plt.scatter(state[0], state[1], color="green", s=100, edgecolors='green') # free sample # for state in collisionchecker.states_bad: # plt.scatter(state[0], state[1], color="red", s=100, edgecolors='red') # collision sample plt.scatter(start()[0], start()[1], color="blue", s=250, edgecolors='black') # init plt.scatter(goal()[0], goal()[1], color="red", s=250, edgecolors='black') # goal plt.xlabel('x') plt.ylabel('y') plt.show()
def graph(self, with_p=False): planner = self.ss.getPlanner() pd = ob.PlannerData(self.ss.getSpaceInformation()) planner.getPlannerData(pd) g = nx.read_graphml(io.StringIO(pd.printGraphML())) traversable = self.map.traversable for n, data in g.nodes(data=True): x = tuple(map(float, data['coords'].split(',')[:3])) data['pos'] = x for n1, n2, data in list(g.edges(data=True)): x = g.node[n1]['pos'] y = g.node[n2]['pos'] v, omega = control(x, y) data['v'] = v data['omega'] = omega if with_p: data['p'], _ = traversable(x, (v, omega)) return g
def graph(self): ros = self.map.to_ros fros = self.map.from_ros planner = self.ss.getPlanner() pd = ob.PlannerData(self.ss.getSpaceInformation()) planner.getPlannerData(pd) g = nx.read_graphml(io.StringIO(pd.printGraphML())) for n, data in g.nodes(data=True): x = list(map(float, data['coords'].split(',')[:2])) data['pos'] = x data['e_pos'] = ros(*x, with_z=True) traversable = self.map.traversable for s, t, data in g.edges(data=True): sx, sy = g.node[s]['pos'] tx, ty = g.node[t]['pos'] p, d = traversable((sx, sy, self.theta), (tx, ty), frame='abs') data['probability'] = p data['duration'] = d return g
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) drawVertex(v) Ve = pd.numEdges() for i in range(0, Vn): for j in range(0, Vn): if pd.edgeExists(i, j): v1 = pd.getVertex(i) v2 = pd.getVertex(j) drawEdge(v1, v2)
def solve(problem): """ Given an instance of the Problem class, containing the problem configuration data, solves the motion planning problem and returns either the solution path or a failure message. """ # Sets up the robot type related information ompl_setup = setup(problem) # Load the planner space_info = ompl_setup.getSpaceInformation() if problem['planner'].startswith('ompl.control.Syclop'): decomposition = ompl_setup.allocDecomposition() planner = eval('%s(space_info, decomposition)' % problem['planner']) ompl_setup.setPlanner(planner) else: planner = eval("%s(space_info)" % problem['planner']) ompl_setup.setPlanner(planner) # Set the optimization objective objectives = {'length': 'PathLengthOptimizationObjective', \ 'max_min_clearance': 'MaximizeMinClearanceObjective', \ 'mechanical_work': 'MechanicalWorkOptimizationObjective'} objective = objectives[problem['objective']] obj = eval('ob.%s(space_info)' % objective) cost = ob.Cost(float(problem['objective.threshold'])) obj.setCostThreshold(cost) ompl_setup.setOptimizationObjective(obj) # Set each parameter that was configured by the user for param in problem['planner_params']: planner.params().setParam(str(param), str(problem['planner_params'][param])) # Solve the problem solution = {} solved = ompl_setup.solve(float(problem['solve_time'])) # Check for validity if solved: if problem['isGeometric']: path = ompl_setup.getSolutionPath() initialValid = path.check() if initialValid: # If initially valid, attempt to simplify ompl_setup.simplifySolution() # Get the simplified path simple_path = ompl_setup.getSolutionPath() simplifyValid = simple_path.check() if simplifyValid: path = simple_path else: OMPL_ERROR("Simplified path was invalid.") else: OMPL_ERROR("Invalid solution path.") else: path = ompl_setup.getSolutionPath().asGeometric() OMPL_INFORM("Path simplification skipped due to non rigid body.") # Interpolate path ns = int(100.0 * float(path.length()) / \ float(ompl_setup.getStateSpace().getMaximumExtent())) if problem["isGeometric"] and len(path.getStates()) < ns: path.interpolate(ns) if len(path.getStates()) != ns: OMPL_WARN("Interpolation produced " + str(len(path.getStates())) + \ " states instead of " + str(ns) + " states.") solution = format_solution(path, True) else: solution = format_solution(None, False) solution['name'] = str(problem['name']) solution['planner'] = ompl_setup.getPlanner().getName() solution['status'] = str(solved) # Store the planner data pd = ob.PlannerData(ompl_setup.getSpaceInformation()) ompl_setup.getPlannerData(pd) explored_states = [] for i in range(0, pd.numVertices()): coords = [] state = pd.getVertex(i).getState() coords.append(state.getX()) coords.append(state.getY()) if problem["is3D"]: coords.append(state.getZ()) else: coords.append(0) explored_states.insert(i, coords) solution["explored_states"] = explored_states return solution
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 plan(self, planning_query: PlanningQuery): self.cleanup_before_plan(planning_query.seed) self.environment = planning_query.environment self.goal_region = self.scenario_ompl.make_goal_region( self.si, rng=self.goal_sampler_rng, params=self.params, goal=planning_query.goal, plot=self.verbose >= 2) # create start and goal states start_state = planning_query.start start_state['stdev'] = np.array([0.0]) start_state['num_diverged'] = np.array([0.0]) self.start_state = start_state ompl_start_scoped = ob.State(self.state_space) self.scenario_ompl.numpy_to_ompl_state(start_state, ompl_start_scoped()) # visualization self.scenario.reset_planning_viz() self.scenario.plot_environment_rviz(planning_query.environment) self.scenario.plot_start_state(start_state) self.scenario.plot_goal_rviz(planning_query.goal, self.params['goal_params']['threshold']) self.ss.clear() self.ss.setStartState(ompl_start_scoped) self.ss.setGoal(self.goal_region) self.ptc = TimeoutOrNotProgressing(self, self.params['termination_criteria'], self.verbose) # START TIMING t0 = time.time() # acutally run the planner ob_planner_status = self.ss.solve(self.ptc) # END TIMING planning_time = time.time() - t0 # handle results and cleanup planner_status = interpret_planner_status(ob_planner_status, self.ptc) if planner_status == MyPlannerStatus.Solved: ompl_path = self.ss.getSolutionPath() actions, planned_path = self.convert_path(ompl_path) planner_data = ob.PlannerData(self.si) self.rrt.getPlannerData(planner_data) tree = planner_data_to_json(planner_data, self.scenario_ompl) elif planner_status == MyPlannerStatus.Timeout: # Use the approximate solution, since it's usually pretty darn close, and sometimes # our goals are impossible to reach so this is important to have try: ompl_path = self.ss.getSolutionPath() actions, planned_path = self.convert_path(ompl_path) except RuntimeError: rospy.logerr( "Timeout before any edges were added. Considering this as Not Progressing." ) planner_status = MyPlannerStatus.NotProgressing actions = [] tree = {} planned_path = [start_state] else: # if no exception was raised planner_data = ob.PlannerData(self.si) self.rrt.getPlannerData(planner_data) tree = planner_data_to_json(planner_data, self.scenario_ompl) elif planner_status == MyPlannerStatus.Failure: rospy.logerr(f"Failed at starting state: {start_state}") tree = {} actions = [] planned_path = [start_state] elif planner_status == MyPlannerStatus.NotProgressing: tree = {} actions = [] planned_path = [start_state] print() return PlanningResult(status=planner_status, path=planned_path, actions=actions, time=planning_time, tree=tree)
pcp.set_alpha(0.7) pcc.set_linestyle('dashed') pcp.set_linestyle('dashed') pcc.set_edgecolor('#73cdc9') pcp.set_edgecolor('#73cdc9') pcc.set_linewidth(0.8) pcp.set_linewidth(0.8) pcc.set_joinstyle('round') pcp.set_joinstyle('round') else: pcc.set_color('gray') pcp.set_color('gray') ax.add_collection(pcc) ax.add_collection(pcp) if args.plannerdata: pd = ob.PlannerData(ob.SpaceInformation(ob.RealVectorStateSpace(2))) pds = ob.PlannerDataStorage() pds.load(args.plannerdata, pd) pd.computeEdgeWeights() # Extract the graphml representation of the planner data graphml = pd.printGraphML() f = open("graph.graphml", 'w') f.write(graphml) f.close() # Load the graphml data using graph-tool graph = gt.load_graph("graph.graphml", fmt="xml") os.remove("graph.graphml") edgeweights = graph.edge_properties["weight"]