def plan(): # create an R^3 state space space = ob.RealVectorStateSpace(3) # set lower and upper bounds bounds = ob.RealVectorBounds(3) bounds.setLow(-1) bounds.setHigh(1) space.setBounds(bounds) # create a simple setup object ss = og.SimpleSetup(space) ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) start = ob.State(space) start()[0] = -1. start()[1] = -1. start()[2] = -1. goal = ob.State(space) goal()[0] = 1. goal()[1] = 1. goal()[2] = 1. ss.setStartAndGoalStates(start, goal, .05) # set the planner planner = RandomWalkPlanner(ss.getSpaceInformation()) ss.setPlanner(planner) result = ss.solve(10.0) if result: if result.getStatus() == ob.PlannerStatus.APPROXIMATE_SOLUTION: print("Solution is approximate") # try to shorten the path ss.simplifySolution() # print the simplified path print(ss.getSolutionPath())
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 _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, 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 __init__(self): ## add OMPL setting for different environments self.env_name = rospy.get_param('env_name') self.planner = rospy.get_param('planner_name') if self.env_name == 's2d': #data_loader = data_loader_2d IsInCollision = plan_s2d.IsInCollision # create an SE2 state space space = ob.RealVectorStateSpace(2) bounds = ob.RealVectorBounds(2) bounds.setLow(-20) bounds.setHigh(20) space.setBounds(bounds) elif self.env_name == 'c2d': #data_loader = data_loader_2d IsInCollision = plan_c2d.IsInCollision # create an SE2 state space space = ob.RealVectorStateSpace(2) bounds = ob.RealVectorBounds(2) bounds.setLow(-20) bounds.setHigh(20) space.setBounds(bounds) elif self.env_name == 'r2d': #data_loader = data_loader_r2d IsInCollision = plan_r2d.IsInCollision # create an SE2 state space space = ob.SE2StateSpace() bounds = ob.RealVectorBounds(2) bounds.setLow(-20) bounds.setHigh(20) space.setBounds(bounds) elif self.env_name == 'r3d': #data_loader = data_loader_r3d IsInCollision = plan_r3d.IsInCollision # create an SE2 state space space = ob.RealVectorStateSpace(3) bounds = ob.RealVectorBounds(3) bounds.setLow(-20) bounds.setHigh(20) space.setBounds(bounds) self.IsInCollision = IsInCollision self.space = space
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 rrt_star_traj(start_conf, goal_conf, env_params, env, robot, planner_params, obs_params): #RRTstar setup space = ob.RealVectorStateSpace(2) bounds = ob.RealVectorBounds(2) bounds.setLow(env_params['x_lims'][0]) bounds.setHigh(env_params['x_lims'][1]) space.setBounds(bounds) init_planner = RRTStar(space, bounds, env, robot, planner_params, 0.0) init_path = init_planner.plan(start_conf, goal_conf, 4.0) th_init = path_to_traj_avg_vel(init_path, planner_params['total_time_sec'], planner_params['dof']) return th_init
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 initialize_space(): dof = 7 space = ob.RealVectorStateSpace(dof) # set the bounds bounds = ob.RealVectorBounds(dof) for key, value in JOINT_LIMITS.items(): i = JOINT_INDEX[key] bounds.setLow(i, value[0]) bounds.setHigh(i, value[1]) print("Setting bound for the %sth joint: %s, bound: %s" % (i, key, value)) space.setBounds(bounds) return space
def setup_ompl(self): self.space = ob.RealVectorStateSpace(dim=self.space_dimension) bounds = ob.RealVectorBounds(self.space_dimension) for i in xrange(self.space_dimension): bounds.setLow(i, self.joint_constraints[0]) bounds.setHigh(i, self.joint_constraints[1]) self.space.setBounds(bounds) self.si = ob.SpaceInformation(self.space) 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) #self.si.setStateValidityChecker(self.motion_validator.isValid) self.si.setMotionValidator(self.motion_validator) self.si.setup() self.problem_definition = ob.ProblemDefinition(self.si)
def __init__(self, ppm_file): self.ppm_ = ou.PPM() self.ppm_.loadFile(ppm_file) space = ob.RealVectorStateSpace() space.addDimension(0.0, self.ppm_.getWidth()) space.addDimension(0.0, self.ppm_.getHeight()) self.maxWidth_ = self.ppm_.getWidth() - 1 self.maxHeight_ = self.ppm_.getHeight() - 1 self.ss_ = og.SimpleSetup(space) # set state validity checking for this space self.ss_.setStateValidityChecker(ob.StateValidityCheckerFn( partial(Plane2DEnvironment.isStateValid, self))) space.setup() self.ss_.getSpaceInformation().setStateValidityCheckingResolution( \ 1.0 / space.getMaximumExtent())
def plan(): N = 10 # create an R^2 state space space = ob.RealVectorStateSpace(2) # set lower and upper bounds bounds = ob.RealVectorBounds(2) bounds.setLow(0) bounds.setHigh(N) space.setBounds(bounds) # create a simple setup object ss = og.SimpleSetup(space) ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) start = ob.State(space) start[0] = 0 # random.randint(0, int(N / 2)) start[1] = 0 # random.randint(0, int(N / 2)) goal = ob.State(space) goal[0] = N # random.randint(int(N / 2), N) goal[1] = N # random.randint(int(N / 2), N) ss.setStartAndGoalStates(start, goal) planner = Astar(ss.getSpaceInformation()) ss.setPlanner(planner) result = ss.solve(.01) if result: if result.getStatus() == ob.PlannerStatus.APPROXIMATE_SOLUTION: print("Solution is approximate") matrix = ss.getSolutionPath().printAsMatrix() print(matrix) verts = [] for line in matrix.split("\n"): x = [] for item in line.split(): x.append(float(item)) if len(x) is not 0: verts.append(list(x)) # print(verts) plt.axis([0, N, 0, N]) x = [] y = [] for i in range(0, len(verts)): x.append(verts[i][0]) y.append(verts[i][1]) # plt.plot(verts[i][0], verts[i][1], 'r*-') plt.plot(x, y, 'ro-') plt.show()
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 rrt_star_traj(start_conf, goal_conf, env_params, env, robot, planner_params, obs_params): #RRTstar setup space = ob.RealVectorStateSpace(2) bounds = ob.RealVectorBounds(2) bounds.setLow(env_params['x_lims'][0]) bounds.setHigh(env_params['x_lims'][1]) space.setBounds(bounds) init_planner = RRTStar(space, bounds, env, robot, planner_params, obs_params) init_path = init_planner.plan(start_conf, goal_conf, 4.0) th_init = path_to_traj_avg_vel(init_path, planner_params['total_time_sec'], planner_params['dof'], device) start_vel = torch.tensor([[0., 0.]], device=device) goal_vel = torch.tensor([[0., 0.]], device=device) start = torch.cat((start_conf, start_vel), dim=1) goal = torch.cat((goal_conf, goal_vel), dim=1) return start, goal, th_init
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 __init__(self, robot_env: DifferentialDriveEnv): self.robot_env = robot_env bounds = self.robot_env.get_bounds() self.state_bounds = bounds['state_bounds'] self.control_bounds = bounds['control_bounds'] # add the state space space = ob.RealVectorStateSpace(len(self.state_bounds)) bounds = ob.RealVectorBounds(len(self.state_bounds)) # add state bounds for k, v in enumerate(self.state_bounds): bounds.setLow(k, float(v[0])) bounds.setHigh(k, float(v[1])) space.setBounds(bounds) self.space = space # add the control space cspace = oc.RealVectorControlSpace(space, len(self.control_bounds)) # set the bounds for the control space cbounds = ob.RealVectorBounds(len(self.control_bounds)) for k, v in enumerate(self.control_bounds): cbounds.setLow(k, float(v[0])) cbounds.setHigh(k, float(v[1])) cspace.setBounds(cbounds) self.cspace = cspace # define a simple setup class self.ss = oc.SimpleSetup(cspace) self.ss.setStateValidityChecker(ob.StateValidityCheckerFn( partial(self.isStateValid, self.ss.getSpaceInformation()))) self.ss.setStatePropagator(oc.StatePropagatorFn(self.propagate)) # set the planner si = self.ss.getSpaceInformation() planner = oc.SST(si) self.ss.setPlanner(planner) si.setPropagationStepSize(.1) self.start = None self.goal = None self.planning_time = 0.0 self.path = None
def plan(): # create an R^3 state space space = ob.RealVectorStateSpace(2) # set lower and upper bounds bounds = ob.RealVectorBounds(2) bounds.setLow(-2.5) bounds.setHigh(2.5) space.setBounds(bounds) # create a simple setup object ss = og.SimpleSetup(space) ss.setStateValidityChecker(ValidityChecker(ss.getSpaceInformation())) start = ob.State(space) start()[0] = 0.0 start()[1] = -2.0 goal = ob.State(space) goal()[0] = 0.0 goal()[1] = 2.0 ss.setStartAndGoalStates(start, goal, .05) # set the planner planner = DPMPPlanner(ss.getSpaceInformation(), 15, 50) ss.setPlanner(planner) result = ss.solve(10.0) if result: if result.getStatus() == ob.PlannerStatus.APPROXIMATE_SOLUTION: print("Solution is approximate") # try to shorten the path # ss.simplifySolution() path = ss.getSolutionPath() print(path) plot_solution(path)
def test_convert_state_to_robot_state(): space = ob.RealVectorStateSpace(7) state = ob.State(space) state[0] = 0 state[1] = -0.55 state[2] = 0 state[3] = 0.75 state[4] = 0 state[5] = 1.26 state[6] = 0 # print("The list of states is: %s" % state) robot_state = convertStateToRobotState(state) expected_joint_names = [ 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2' ] expected_joint_values = [0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0] assert robot_state.joint_state.name == ['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2'],\ "joint names do not match! Expected: %s, Actual %s" % (expected_joint_names, robot_state.joint_state.name) assert robot_state.joint_state.position == expected_joint_values, \ "joint values do not match! Expected: %s, Actual %s" % (expected_joint_values, robot_state.joint_state.position) print("Test for converting AbstractState to RobotState passed!")
def __init__(self, map, pose, target, max_time, objective='duration', planner=og.RRTstar, threshold=0.9, tolerance=0.3, planner_options={'range': 0.45}): space = ob.RealVectorStateSpace(2) bounds = ob.RealVectorBounds(2) bounds.setLow(0) bounds.setHigh(map.size) space.setBounds(bounds) s = ob.State(space) t = ob.State(space) self.theta = pose[-1] for arg, state in zip([pose[:2], target], [s, t]): for i, x in enumerate(arg): state[i] = x ss = og.SimpleSetup(space) ss.setStartAndGoalStates(s, t, tolerance) si = ss.getSpaceInformation() ss.setStateValidityChecker( ob.StateValidityCheckerFn(partial(valid, si))) self.motion_val = EstimatedMotionValidator(si, map, threshold, theta=self.theta) si.setMotionValidator(self.motion_val) if objective == 'duration': self.do = DurationObjective(si, map, self.theta) elif objective == 'survival': self.do = SurvivalObjective(si, map, self.theta) elif objective == 'balanced': self.do = getBalancedObjective1(si, map, self.theta) else: # the objective is the euclidean path length self.do = ob.PathLengthOptimizationObjective(si) ss.setOptimizationObjective(self.do) # RRTstar BITstar BFMT RRTsharp p = planner(si) if 'range' in planner_options: try: p.setRange(planner_options.get('range')) except AttributeError: pass ss.setPlanner(p) ss.setup() self.ss = ss self.max_time = max_time self.map = map self.s = pose self.t = list(target) + [pose[-1]] self.planner_options = planner_options if planner == og.RRTstar: self.planner_name = 'RRT*' else: self.planner_name = '' self.threshold = threshold self.tolerance = tolerance self.comp_duration = 0 self.objective = objective
def isStateValid_R2(state): return boxConstraint(state[0], state[1]) if __name__ == "__main__": # Setup SE2 SE2 = ob.SE2StateSpace() bounds = ob.RealVectorBounds(2) bounds.setLow(0) bounds.setHigh(1) SE2.setBounds(bounds) si_SE2 = ob.SpaceInformation(SE2) si_SE2.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid_SE2)) # Setup Quotient-Space R2 R2 = ob.RealVectorStateSpace(2) R2.setBounds(0, 1) si_R2 = ob.SpaceInformation(R2) 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)
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"]
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()
# dis = abs(st1[0] - st0[0]) + abs(st1[1]-st0[1]) # print(dis) # dis = 1 return float(dis) # fmt_star sucess = 0 total = 0 total_length = 0 total_collision_detection = 0 total_collision = 0 for i in range(0,2): print("here") # state space space = ob.RealVectorStateSpace(4) space = myStateSpace() # space.distance = myDistance # set lower and upper bounds bounds = ob.RealVectorBounds(4) bounds.setLow(0, 0) bounds.setHigh(0, 1) bounds.setLow(1, 0) bounds.setHigh(1, 1) bounds.setLow(2, -1) bounds.setHigh(2, 1) bounds.setLow(3, -1) bounds.setHigh(3, 1) space.setBounds(bounds) # construct an instance of space information from this state space si = ob.SpaceInformation(space)
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 solve(self, ptc): pdef = self.getProblemDefinition() goal = pdef.getGoal() si = self.getSpaceInformation() pi = self.getPlannerInputStates() st = pi.nextStart() while st: self.states_.append(st) st = pi.nextStart() solution = None approxsol = 0 approxdif = 1e6 start_state = pdef.getStartState(0) goal_state = goal.getState() start_node = Node(None, start_state) start_node.g = start_state.h = start_node.f = 0 end_node = Node(None, goal_state) end_node.g = end_node.h = end_node.f = 0 open_list = [] closed_list = [] heapq.heapify(open_list) adjacent_squares = ((0, -1, 3 * math.pi / 2), (0, 1, math.pi / 2), (-1, 0, math.pi), (1, 0, 0), (-1, -1, 5 * math.pi / 4), (-1, 1, 3 * math.pi / 2), (1, -1, 7 * math.pi / 4), (1, 1, math.pi / 4)) heapq.heappush(open_list, start_node) while len(open_list) > 0 and not ptc(): current_node = heapq.heappop(open_list) if current_node == end_node: # if we hit the goal current = current_node path = [] while current is not None: path.append(current.position) current = current.parent for i in range(1, len(path)): self.states_.append(path[len(path) - i - 1]) solution = len(self.states_) break closed_list.append(current_node) children = [] for new_position in adjacent_squares: node_position = si.allocState() # node_position = ob.State() if isinstance(si.getStateSpace(), type(ob.ReedsSheppStateSpace(6))): current_node_x = current_node.position.getX() current_node_y = current_node.position.getY() node_position.setXY(current_node_x + new_position[0], current_node_y + new_position[1]) node_position.setYaw(new_position[2]) if isinstance(si.getStateSpace(), type(ob.RealVectorStateSpace())): node_position[0], node_position[1] = current_node.position[0] + new_position[0],\ current_node.position[1] + new_position[1] # print(node_position.getX(), node_position.getY(), node_position.getYaw()) # node_position[0], node_position[1] = current_node_x + new_position[0], current_node_y + new_position[1] if not si.checkMotion(current_node.position, node_position): continue if not si.satisfiesBounds(node_position): continue new_node = Node(current_node, node_position) children.append(new_node) for child in children: if child in closed_list: continue child.g = current_node.g + 1 # si.distance(child.position, current_node.position) child.h = goal.distanceGoal(child.position) child.f = child.g + child.h if len([i for i in open_list if child == i and child.g >= i.g ]) > 0: continue heapq.heappush(open_list, child) # open_list.append(child) solved = False approximate = False if not solution: solution = approxsol approximate = True if solution: path = og.PathGeometric(si) for s in self.states_[:solution]: path.append(s) pdef.addSolutionPath(path) solved = True return ob.PlannerStatus(solved, approximate)
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(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 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.")