def __init__(self, ppm_file, obs: list, start=(1, 1), goal=(1000, 1000)): self.ppm_ = ou.PPM() self.ppm_.loadFile(ppm_file) self.width = self.ppm_.getWidth() self.height = self.ppm_.getHeight() self.space = ob.RealVectorStateSpace() self.space.addDimension(0.0, self.width) self.space.addDimension(0.0, self.height) self.bounds = ob.RealVectorBounds(2) self.bounds.setLow(0) self.bounds.setHigh(1024) self.space.setBounds(self.bounds) self.cspace = oc.RealVectorControlSpace(self.space, 2) self.cbounds = ob.RealVectorBounds(2) self.cbounds.setLow(-1) self.cbounds.setHigh(1) self.cspace.setBounds(self.cbounds) self.setup = oc.SimpleSetup(self.cspace) self.obs = obs self.start = start self.goal = goal
def plan(): # construct the state space we are planning in space = ob.SE2StateSpace() # set the bounds for the R^2 part of SE(2) bounds = ob.RealVectorBounds(2) bounds.setLow(-1) bounds.setHigh(1) space.setBounds(bounds) # create a control space cspace = oc.RealVectorControlSpace(space, 2) # set the bounds for the control space cbounds = ob.RealVectorBounds(2) cbounds.setLow(-.3) cbounds.setHigh(.3) cspace.setBounds(cbounds) # define a simple setup class ss = oc.SimpleSetup(cspace) ss.setStateValidityChecker(ob.StateValidityCheckerFn( \ partial(isStateValid, ss.getSpaceInformation()))) ss.setStatePropagator(oc.StatePropagatorFn(propagate)) # create a start state start = ob.State(space) start().setX(-0.5) start().setY(0.0) start().setYaw(0.0) # create a goal state goal = ob.State(space) goal().setX(0.0) goal().setY(0.5) goal().setYaw(0.0) # set the start and goal states ss.setStartAndGoalStates(start, goal, 0.05) # (optionally) set planner si = ss.getSpaceInformation() #planner = oc.RRT(si) #planner = oc.EST(si) #planner = oc.KPIECE1(si) # this is the default # SyclopEST and SyclopRRT require a decomposition to guide the search decomp = MyDecomposition(32, bounds) planner = oc.SyclopEST(si, decomp) #planner = oc.SyclopRRT(si, decomp) ss.setPlanner(planner) # (optionally) set propagation step size si.setPropagationStepSize(.1) # attempt to solve the problem solved = ss.solve(20.0) if solved: # print the path to screen print("Found solution:\n%s" % ss.getSolutionPath().printAsMatrix())
def plan(): # construct the state space we are planning in # The state space of the car is SE(2) (x and y position with one angle for orientation). space = ob.SE2StateSpace() # set the bounds for the R^2 part of SE(2) bounds = ob.RealVectorBounds(2) bounds.setLow(-1) bounds.setHigh(1) space.setBounds(bounds) # create a control space # ?? for this simple car model consists of the velocity and steering angle, both real valued. cspace = oc.RealVectorControlSpace(space, 2) # set the bounds for the control space cbounds = ob.RealVectorBounds(2) cbounds.setLow(-.3) cbounds.setHigh(.3) cspace.setBounds(cbounds) # define a simple setup class global ss ss = oc.SimpleSetup(cspace) validityChecker = ob.StateValidityCheckerFn(partial(isStateValid, ss.getSpaceInformation())) ss.setStateValidityChecker(validityChecker) ode = oc.ODE(kinematicCarODE) #odeSolver = oc.ODEBasicSolver(ss.getSpaceInformation(), ode) odeSolver = oc.ODEErrorSolver(ss.getSpaceInformation(), ode) propagator = oc.ODESolver.getStatePropagator(odeSolver) ss.setStatePropagator(propagator) # create a start state start = ob.State(space) start().setX(-0.5) start().setY(0.0) start().setYaw(0.0) # create a goal state goal = ob.State(space) goal().setX(0.0) goal().setY(0.5) goal().setYaw(0.0) # set the start and goal states ss.setStartAndGoalStates(start, goal, 0.05) # attempt to solve the problem solved = ss.solve(120.0) if solved: # print the path to screen print("Found solution:\n%s" % ss.getSolutionPath().printAsMatrix())
def plan(): # construct the state space we are planning in space = ob.SE2StateSpace() # set the bounds for the R^2 part of SE(2) bounds = ob.RealVectorBounds(2) bounds.setLow(-1) bounds.setHigh(1) space.setBounds(bounds) # create a control space cspace = oc.RealVectorControlSpace(space, 2) # set the bounds for the control space cbounds = ob.RealVectorBounds(2) cbounds.setLow(-.3) cbounds.setHigh(.3) cspace.setBounds(cbounds) # define a simple setup class ss = oc.SimpleSetup(cspace) validityChecker = ob.StateValidityCheckerFn( partial(isStateValid, ss.getSpaceInformation())) ss.setStateValidityChecker(validityChecker) ode = oc.ODE(kinematicCarODE) odeSolver = oc.ODEBasicSolver(ss.getSpaceInformation(), ode) propagator = oc.ODESolver.getStatePropagator(odeSolver) ss.setStatePropagator(propagator) # create a start state start = ob.State(space) start().setX(-0.5) start().setY(0.0) start().setYaw(0.0) # create a goal state goal = ob.State(space) goal().setX(0.0) goal().setY(0.5) goal().setYaw(0.0) # set the start and goal states ss.setStartAndGoalStates(start, goal, 0.05) # attempt to solve the problem solved = ss.solve(120.0) if solved: # print the path to screen print("Found solution:\n%s" % ss.getSolutionPath().asGeometric().printAsMatrix())
def setSpace(self): # construct the state space we are planning in self.space = ob.SE2StateSpace() # set the bounds for the R^2 part of SE(2) self.bounds = ob.RealVectorBounds(2) if not self.costMap: self.bounds.setLow(-8) self.bounds.setHigh(20) else: ox = self.costMap.getOriginX() oy = self.costMap.getOriginY() size_x = self.costMap.getSizeInMetersX() size_y = self.costMap.getSizeInMetersY() low = min(ox, oy) high = max(ox + size_x, oy + size_y) print("low", low) print("high", high) #self.bounds.setLow(low) #self.bounds.setHigh(high) self.bounds.setLow(0, ox) self.bounds.setHigh(0, ox + size_x) self.bounds.setLow(1, oy) self.bounds.setHigh(1, oy + size_y) self.space.setBounds(self.bounds) # create a control space self.cspace = oc.RealVectorControlSpace(self.space, 2) # set the bounds for the control space cbounds = ob.RealVectorBounds(2) cbounds.setLow(0, -1.5) cbounds.setHigh(0, 1.5) cbounds.setLow(1, -3) cbounds.setHigh(1, 3) self.cspace.setBounds(cbounds) # define a simple setup class self.ss = oc.SimpleSetup(self.cspace) self.ss.setStateValidityChecker( ob.StateValidityCheckerFn( partial(self.isStateValid, self.ss.getSpaceInformation()))) self.ss.setStatePropagator(oc.StatePropagatorFn(self.propagate))
def setSpace(self): self.space = ob.SE2StateSpace() # set the bounds for the R^2 part of SE(2) self.bounds = ob.RealVectorBounds(2) if not self.costMap: self.bounds.setLow(-8) self.bounds.setHigh(20) else: ox = self.costMap.getOriginX() oy = self.costMap.getOriginY() size_x = self.costMap.getSizeInMetersX() size_y = self.costMap.getSizeInMetersY() low = min(ox, oy) high = max(ox + size_x, oy + size_y) print("low", low) print("high", high) self.bounds.setLow(low) self.bounds.setHigh(high) self.space.setBounds(self.bounds) # create a control space self.cspace = oc.RealVectorControlSpace(self.space, 2) # set the bounds for the control space cbounds = ob.RealVectorBounds(2) cbounds.setLow(0, -1.5) cbounds.setHigh(0, 1.5) cbounds.setLow(1, -3) cbounds.setHigh(1, 3) self.cspace.setBounds(cbounds) # define a simple setup class self.ss = oc.SimpleSetup(self.cspace) validityChecker = ob.StateValidityCheckerFn( partial(self.isStateValid, self.ss.getSpaceInformation())) self.ss.setStateValidityChecker(validityChecker) ode = oc.ODE(self.kinematicCarODE) odeSolver = oc.ODEBasicSolver(self.ss.getSpaceInformation(), ode) propagator = oc.ODESolver.getStatePropagator(odeSolver) self.ss.setStatePropagator(propagator)
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 __init__(self, ndof, iksolver, step_size=0.05): self.ndof = ndof self.iksolver = iksolver self.state_space = MyStateSpace(ndof) self.control_space = MyControlSpace(self.state_space, ndof) self.simple_setup = oc.SimpleSetup(self.control_space) si = self.simple_setup.getSpaceInformation() si.setPropagationStepSize(step_size) si.setMinMaxControlDuration(1, 1) si.setDirectedControlSamplerAllocator( oc.DirectedControlSamplerAllocator( directedControlSamplerAllocator)) vc = MyStateValidityChecker(self.simple_setup.getSpaceInformation(), self.iksolver, ndof) self.simple_setup.setStateValidityChecker(vc) ob = MyOptimizationObjective(self.simple_setup.getSpaceInformation()) self.simple_setup.setOptimizationObjective(ob) propagator = MyStatePropagator(self.simple_setup.getSpaceInformation(), ndof) self.simple_setup.setStatePropagator(propagator) # self.planner = oc.KPIECE1(self.simple_setup.getSpaceInformation()) # self.planner.setup() # ========= RRT planner ============ self.planner = og.RRTstar(self.simple_setup.getSpaceInformation()) p_goal = 0.0 self.planner.setGoalBias(p_goal) self.planner.setRange(step_size) IPython.embed() # =========== PRM planner ============= # self.planner = og.PRM(self.simple_setup.getSpaceInformation()) self.simple_setup.setPlanner(self.planner)
def plan(): # construct the state space we are planning in ripl = app.SE2RigidBodyPlanning() ripl.setEnvironmentMesh('./mesh/UniqueSolutionMaze_env.dae') ripl.setRobotMesh('./mesh/car2_planar_robot.dae') # space = ob.SE2StateSpace() space = ripl.getStateSpace() # set the bounds for the R^2 part of SE(2) bounds = ob.RealVectorBounds(2) bounds.setLow(-55) bounds.setHigh(55) space.setBounds(bounds) # create a control space cspace = oc.RealVectorControlSpace(space, 2) # set the bounds for the control space cbounds = ob.RealVectorBounds(2) cbounds.setLow(0, -3.0) cbounds.setHigh(0, 3.0) cbounds.setLow(1, 3.0) cbounds.setHigh(1, 3.0) cspace.setBounds(cbounds) # define a simple setup class ss = oc.SimpleSetup(cspace) # ss.setStateValidityChecker(ob.StateValidityCheckerFn( \ # partial(isStateValid, ss.getSpaceInformation()))) sp_info = ss.getSpaceInformation() extractor = ripl.getGeometricStateExtractor() enabled = ripl.isSelfCollisionEnabled() checker = ripl.allocStateValidityChecker(sp_info, extractor, enabled) ss.setStateValidityChecker(checker) ss.setStatePropagator(oc.StatePropagatorFn(propagate)) # create a start state start = ob.State(space) start().setX(3) start().setY(-3) start().setYaw(0.0) # create a goal state goal = ob.State(space) goal().setX(45) goal().setY(25) goal().setYaw(0.0) # set the start and goal states ss.setStartAndGoalStates(start, goal, 0.05) # (optionally) set planner si = ss.getSpaceInformation() # planner = oc.PDST(si) # planner = oc.RRT(si) # planner = oc.EST(si) planner = oc.KPIECE1(si) # this is the default # SyclopEST and SyclopRRT require a decomposition to guide the search decomp = MyDecomposition(32, bounds) # planner = oc.SyclopEST(si, decomp) # planner = oc.SyclopRRT(si, decomp) ss.setPlanner(planner) # (optionally) set propagation step size si.setPropagationStepSize(.1) # attempt to solve the problem solved = ss.solve(20.0) if solved: # print the path to screen print("Found solution:\n%s" % ss.getSolutionPath().printAsMatrix()) with open('path.txt', 'w') as outFile: outFile.write(ss.getSolutionPath().printAsMatrix())
def execute(self, env, time, pathLength, show = False): result = True sSpace = MyStateSpace() sbounds = ob.RealVectorBounds(4) # dimension 0 (x) spans between [0, width) # dimension 1 (y) spans between [0, height) # since sampling is continuous and we round down, we allow values until # just under the max limit # the resolution is 1.0 since we check cells only sbounds.low = ou.vectorDouble() sbounds.low.extend([0.0, 0.0, -MAX_VELOCITY, -MAX_VELOCITY]) sbounds.high = ou.vectorDouble() sbounds.high.extend([float(env.width) - 0.000000001, float(env.height) - 0.000000001, MAX_VELOCITY, MAX_VELOCITY]) sSpace.setBounds(sbounds) cSpace = oc.RealVectorControlSpace(sSpace, 2) cbounds = ob.RealVectorBounds(2) cbounds.low[0] = -MAX_VELOCITY cbounds.high[0] = MAX_VELOCITY cbounds.low[1] = -MAX_VELOCITY cbounds.high[1] = MAX_VELOCITY cSpace.setBounds(cbounds) ss = oc.SimpleSetup(cSpace) isValidFn = ob.StateValidityCheckerFn(partial(isValid, env.grid)) ss.setStateValidityChecker(isValidFn) propagator = MyStatePropagator(ss.getSpaceInformation()) ss.setStatePropagator(propagator) planner = self.newplanner(ss.getSpaceInformation()) ss.setPlanner(planner) # the initial state start = ob.State(sSpace) start()[0] = env.start[0] start()[1] = env.start[1] start()[2] = 0.0 start()[3] = 0.0 goal = ob.State(sSpace) goal()[0] = env.goal[0] goal()[1] = env.goal[1] goal()[2] = 0.0 goal()[3] = 0.0 ss.setStartAndGoalStates(start, goal, 0.05) startTime = clock() if ss.solve(SOLUTION_TIME): elapsed = clock() - startTime time = time + elapsed if show: print('Found solution in %f seconds!' % elapsed) path = ss.getSolutionPath() path.interpolate() if not path.check(): return (False, time, pathLength) 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 plan(run_time, planner_type, objective_type, wind_direction, dimensions, obstacles): # 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() bounds = ob.RealVectorBounds(2) bounds.setLow(0, dimensions[0]) bounds.setLow(1, dimensions[1]) bounds.setHigh(0, dimensions[2]) bounds.setHigh(1, dimensions[3]) # Set the bounds of space to be in [0,1]. space.setBounds(bounds) # create a control space cspace = oc.RealVectorControlSpace(space, 1) # set the bounds for the control space cbounds = ob.RealVectorBounds(1) cbounds.setLow(-math.pi / 3) cbounds.setHigh(math.pi / 3) cspace.setBounds(cbounds) # define a simple setup class ss = oc.SimpleSetup(cspace) propagator = get_state_propagator(wind_direction) ss.setStatePropagator(oc.StatePropagatorFn(propagator)) # Construct a space information instance for this state space si = ss.getSpaceInformation() si.setPropagationStepSize(1) # Set the object used to check which states in the space are valid validity_checker = ValidityChecker(si, obstacles) ss.setStateValidityChecker(validity_checker) # Set our robot's starting state to be the bottom-left corner of # the environment, or (0,0). start = ob.State(space) start[0] = dimensions[0] start[1] = dimensions[1] start[2] = math.pi / 4 # Set our robot's goal state to be the top-right corner of the # environment, or (1,1). goal = ob.State(space) goal[0] = dimensions[2] goal[1] = dimensions[3] goal[2] = math.pi / 4 # Set the start and goal states ss.setStartAndGoalStates(start, goal) # Create the optimization objective specified by our command-line argument. # This helper function is simply a switch statement. objective = allocate_objective(si, objective_type) ss.setOptimizationObjective(objective) # Create a decomposition of the state space decomp = MyDecomposition(256, bounds) # Construct the optimal planner specified by our command line argument. # This helper function is simply a switch statement. optimizing_planner = allocate_planner(si, planner_type, decomp) ss.setPlanner(optimizing_planner) # attempt to solve the planning problem in the given runtime solved = ss.solve(run_time) 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.getPlanner().getName(), ss.getSolutionPath().length(), 0.1)) print(ss.getSolutionPath().printAsMatrix()) print(ss.haveExactSolutionPath()) plot_path(ss.getSolutionPath(), dimensions, obstacles) else: print("No solution found.")
def __init__( self, fwd_model: BaseDynamicsFunction, filter_model: BaseFilterFunction, classifier_model: BaseConstraintChecker, planner_params: Dict, action_params: Dict, scenario: ExperimentScenario, verbose: int, ): super().__init__(scenario=scenario, fwd_model=fwd_model, filter_model=filter_model) self.verbose = verbose self.fwd_model = fwd_model self.classifier_model = classifier_model self.params = planner_params self.action_params = action_params # TODO: consider making full env params h/w come from elsewhere. res should match the model though. self.si = ob.SpaceInformation(ob.StateSpace()) self.classifier_rng = np.random.RandomState(0) self.state_sampler_rng = np.random.RandomState(0) self.goal_sampler_rng = np.random.RandomState(0) self.control_sampler_rng = np.random.RandomState(0) self.scenario = scenario self.scenario_ompl = get_ompl_scenario(self.scenario) self.state_space = self.scenario_ompl.make_ompl_state_space( planner_params=self.params, state_sampler_rng=self.state_sampler_rng, plot=self.verbose >= 2) # self.state_space.sanityChecks() self.control_space = self.scenario_ompl.make_ompl_control_space( self.state_space, self.control_sampler_rng, action_params=self.action_params) self.ss = oc.SimpleSetup(self.control_space) self.si = self.ss.getSpaceInformation() self.ss.setStatePropagator(oc.AdvancedStatePropagatorFn( self.propagate)) self.ss.setMotionsValidityChecker( oc.MotionsValidityCheckerFn(self.motions_valid)) self.ss.setStateValidityChecker( ob.StateValidityCheckerFn(self.is_valid)) self.cleanup_before_plan(0) # just for debugging self.cc = CollisionCheckerClassifier( [pathlib.Path("cl_trials/cc_baseline/cc")], self.scenario, 0.0) self.cc_but_accept_count = 0 self.rrt = oc.RRT(self.si) self.rrt.setIntermediateStates( True ) # this is necessary, because we use this to generate datasets self.ss.setPlanner(self.rrt) self.si.setMinMaxControlDuration(1, 50)
def __init__(self, map_, pose, target, max_time=120, allow_moving_backward=True, omega_max=0.5, vmax=0.08, objective='duration', planner=oc.SST, threshold=0.9, tolerance=0.3, control_duration=1, k=1.0, min_y=None, max_y=None): space = ob.SE2StateSpace() bounds = ob.RealVectorBounds(2) bounds.setLow(0) bounds.setHigh(map_.size) if min_y is not None: bounds.setLow(min_y) if max_y is not None: bounds.setHigh(max_y) space.setBounds(bounds) cspace = oc.RealVectorControlSpace(space, 2) cbounds = ob.RealVectorBounds(2) cbounds.setLow(-omega_max) cbounds.setHigh(omega_max) cspace.setBounds(cbounds) ss = oc.SimpleSetup(cspace) si = ss.getSpaceInformation() ss.setStateValidityChecker( ob.StateValidityCheckerFn(partial(valid, si))) self.propagation_data = {'t': 0, 'nt': 0} ss.setStatePropagator( oc.StatePropagatorFn( partial(propagate, self.propagation_data, map_, threshold, allow_moving_backward))) start = ob.State(space) state_from_tuple(start(), pose) goal = ob.State(space) state_from_tuple(goal(), target) ss.setStartAndGoalStates(start, goal, tolerance) p = planner(si) ss.setPlanner(p) si.setPropagationStepSize(control_duration) si.setMinMaxControlDuration(control_duration, control_duration) if objective == 'duration': do = DurationObjective(si) elif objective == 'balanced': do = getBalancedObjective1(si, k=k) else: do = ob.PathLengthOptimizationObjective(si) ss.setOptimizationObjective(do) ss.setup() self.ss = ss self.map = map_ self.s = pose self.t = target self.objective = objective self.planner_name = 'SST' self.planner_options = {} self.threshold = threshold self.tolerance = tolerance self.comp_duration = 0 self.max_time = max_time self.allow_moving_backward = allow_moving_backward
def plan(self,goalPoints,current_region,next_region,samplerIndex): """ goal points: array that contains the coordinates of all the possible goal states current_reg: name of the current region (p1 etc) next_reg : name of the next region (p1 etc) """ # construct the state space we are planning in if self.Space_Dimension == 2: space = ob.SE2StateSpace() else: space = ob.SE3StateSpace() # set the bounds bounds = ob.RealVectorBounds(self.Space_Dimension) BoundaryMaxMin = self.all.boundingBox() #0-3:xmin, xmax, ymin and ymax bounds.setLow(0,BoundaryMaxMin[0]) # 0 stands for x axis bounds.setHigh(0,BoundaryMaxMin[1]) bounds.setLow(1,BoundaryMaxMin[2]) # 1 stands for y axis bounds.setHigh(1,BoundaryMaxMin[3]) if self.Space_Dimension == 3: bounds.setLow(2,0) # 2 stands for z axis bounds.setHigh(2,self.maxHeight) space.setBounds(bounds) if self.system_print == True: print "The bounding box of the boundary is: " + str(self.all.boundingBox() ) print "The volume of the bounding box is: " + str(bounds.getVolume()) if self.Geometric_Control == 'C': # create a control space cspace = oc.RealVectorControlSpace(space, self.Space_Dimension) # set the bounds for the control space # cbounds[0] for velocity # cbounds[1] for angular velocity cbounds = ob.RealVectorBounds(self.Space_Dimension) cbounds.setLow(0,0) cbounds.setHigh(0,max((BoundaryMaxMin[1]-BoundaryMaxMin[0]),(BoundaryMaxMin[3]-BoundaryMaxMin[2]))/100) cbounds.setLow(1,-pi/5) cbounds.setHigh(1,pi/5) cspace.setBounds(cbounds) if self.system_print == True: print cspace.settings() if self.Geometric_Control == 'G': # define a simple setup class ss = og.SimpleSetup(space) else: # define a simple setup class ss = oc.SimpleSetup(cspace) # set state validity checking for this space ss.setStatePropagator(oc.StatePropagatorFn(self.propagate)) ss.setStateValidityChecker(ob.StateValidityCheckerFn(self.isStateValid)) # create a start state start = ob.State(space) pose = self.pose_handler.getPose() #x,y,w,(z if using ROS quadrotor) start().setX(pose[0]) start().setY(pose[1]) if self.Space_Dimension == 2: while pose[2] > pi or pose[2] < -pi: if pose[2]> pi: pose[2] = pose[2] - pi else: pose[2] = pose[2] + pi start().setYaw(pose[2]) # else: start().setZ(pose[3]) start().rotation().setIdentity() if self.system_print is True and self.Space_Dimension == 3: print "start:" + str(start().getX())+","+str(start().getY()) +"," + str(start().getZ()) print goalPoints # create goal states goalStates = ob.GoalStates(ss.getSpaceInformation()) for i in range(shape(goalPoints)[1]): goal = ob.State(space) goal().setX(goalPoints[0,i]) goal().setY(goalPoints[1,i]) if self.Space_Dimension == 3: if self.system_print is True: print current_region,next_region print self.map['height'][current_region],self.map['height'][next_region] z_goalPoint = min(self.map['height'][current_region]/2,self.map['height'][next_region]/2) goal().setZ(z_goalPoint) goal().rotation().setIdentity() else: goal().setYaw(0.0) if self.plotting == True: if self.Space_Dimension == 3: self.ax.plot([goalPoints[0,i]],[goalPoints[1,i]],[z_goalPoint],'ro') else: self.ax.plot([goalPoints[0,i]],[goalPoints[1,i]],'ro') self.setPlotLimitXYZ() self.ax.get_figure().canvas.draw() goalStates.addState(goal) if self.system_print is True: print goalStates # set the start and goal states; ss.setGoal(goalStates) ss.setStartState(start) # set sampler (optional; the default is uniform sampling) si = ss.getSpaceInformation() # set planner planner_prep = self.planner_dictionary[self.Geometric_Control][self.planner] planner = planner_prep(si) ss.setPlanner(planner) if self.Geometric_Control == 'G': if not self.planner == 'PRM': planner.setRange(self.radius*2) if self.system_print is True: print "planner.getRange():" + str(planner.getRange()) #if not self.planner == 'RRTConnect': # planner.setGoalBias(0.5) else: # (optionally) set propagation step size si.setPropagationStepSize(1) #actually is the duration in propagate si.setMinMaxControlDuration(3,3) # is the no of steps taken with the same velocity and omega if self.system_print is True: print "radius: " +str(self.radius) print "si.getPropagationStepSize():" + str(si.getPropagationStepSize()) planner.setGoalBias(0.5) ss.setup() # attempt to solve the problem within ten seconds of planning time solved = ss.solve(1000.0) #10 if (solved): print("Found solution:") # print the path to screen print >>sys.__stdout__,(ss.getSolutionPath()) else: print("No solution found") if self.plotting == True : self.ax.set_title('Map with Geo/Control: '+str(self.Geometric_Control) + ",Planner:" +str(self.planner), fontsize=12) self.ax.set_xlabel('x') self.ax.set_ylabel('y') for i in range(ss.getSolutionPath().getStateCount()-1): if self.Space_Dimension == 3: self.ax.plot(((ss.getSolutionPath().getState(i)).getX(),(ss.getSolutionPath().getState(i+1)).getX()),((ss.getSolutionPath().getState(i)).getY(),(ss.getSolutionPath().getState(i+1)).getY()),((ss.getSolutionPath().getState(i)).getZ(),(ss.getSolutionPath().getState(i+1)).getZ()),'b') ro=Polygon.Shapes.Circle (self.radius,((ss.getSolutionPath().getState(i)).getX(),(ss.getSolutionPath().getState(i)).getY())) self.plotPoly(ro,'r') else: self.ax.plot(((ss.getSolutionPath().getState(i)).getX(),(ss.getSolutionPath().getState(i+1)).getX()),((ss.getSolutionPath().getState(i)).getY(),(ss.getSolutionPath().getState(i+1)).getY()),0,'b') self.setPlotLimitXYZ() #self.ax.get_figure().canvas.draw() return ss