def plan(self): start = ob.State(self.space) (start[0], start[1]) = self.start goal = ob.State(self.space) (goal[0], goal[1]) = self.goal def isStateValid(spaceInformation, state): # perform collision checking or check if other constraints are satisfied # state_x, state_y = state[0],state[1] return spaceInformation.satisfiesBounds(state) and \ all([not circle.isInObs(State2D(state)) for circle in self.obs]) def propagate(start, control, duration, state): for i in range(2): state[i] = start[i] + duration * control[i] self.setup.setStateValidityChecker( ob.StateValidityCheckerFn( partial(isStateValid, self.setup.getSpaceInformation()))) self.setup.setStatePropagator(oc.StatePropagatorFn(propagate)) tol = Config.distance_to_goal_tolerance self.setup.setStartAndGoalStates(start, goal, tol) si = self.setup.getSpaceInformation() # planner = oc.KPIECE1(si) planner = oc.RRT(si) self.setup.setPlanner(planner) step_size = Config.propagation_step_size si.setPropagationStepSize(step_size) solution_time = Config.solution_time return self.setup.solve(solution_time)
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 ss = app.KinematicCarPlanning() ss.setEnvironmentMesh('./mesh/UniqueSolutionMaze_env.dae') ss.setRobotMesh('./mesh/car2_planar_robot.dae') # space = ob.SE2StateSpace() cspace = ss.getControlSpace() # 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) # create a start state start = ob.State(ss.getSpaceInformation()) start().setX(3) start().setY(-3) start().setYaw(0.0) # create a goal state goal = ob.State(ss.getSpaceInformation()) goal().setX(45) goal().setY(25) goal().setYaw(0.0) #si = ss.getSpaceInformation() information = ss.getSpaceInformation() information.setStatePropagator(oc.StatePropagatorFn(propagate)) # set the start and goal states ss.setStartAndGoalStates(start, goal, 0.05) # 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 configure(ss): # create a start state cspace = ss.getControlSpace() # set the bounds for the control space cbounds = ob.RealVectorBounds(2) cbounds.setLow(0, 0.0) cbounds.setHigh(0, 3.0) cbounds.setLow(1, 0.0) cbounds.setHigh(1, 3.0) cspace.setBounds(cbounds) si = ss.getSpaceInformation() si.setStatePropagator(oc.StatePropagatorFn(propagate))
def plan(options, params): print(params) """ Entry point for the planning """ # construct the state space we are planning in ss = app.KinematicCarPlanning() ss.setEnvironmentMesh(params["robot_mesh"]) ss.setRobotMesh(params["env_mesh"]) # setting the real vector state space bounds se2bounds = ob.RealVectorBounds(2) se2bounds.setLow(params["lower_bound"]) se2bounds.setHigh(params["upper_bound"]) ss.getStateSpace().setBounds(se2bounds) # set the bounds for the control space cspace = ss.getControlSpace() cbounds = ob.RealVectorBounds(2) cbounds.setLow(0, -3.0) cbounds.setHigh(0, 3.0) cbounds.setLow(1, params["lower_bound_cspace"]) cbounds.setHigh(1, 3.0) cspace.setBounds(cbounds) # create a start state start = ob.State(ss.getSpaceInformation()) start().setX(params["start_x"]) start().setY(params["start_y"]) start().setYaw(0.0) # create a goal state goal = ob.State(ss.getSpaceInformation()) goal().setX(params["goal_x"]) goal().setY(params["goal_y"]) goal().setYaw(0.0) # space information si = ss.getSpaceInformation() si.setStatePropagator(oc.StatePropagatorFn(propagate)) # set the start and goal states ss.setStartAndGoalStates(start, goal, 0.05) if not options.bench: planOnce(ss) else: benchmark(ss)
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 __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(): # 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 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, 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