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.space.setBounds(self.bounds) # define a simple setup class self.ss = og.SimpleSetup(self.space) self.ss.setStateValidityChecker( ob.StateValidityCheckerFn(self.isStateValid))
def plan(): # create an SE2 state space space = ob.SE2StateSpace() # set lower and upper bounds bounds = ob.RealVectorBounds(2) bounds.setLow(-1) bounds.setHigh(1) space.setBounds(bounds) # create a simple setup object ss = og.SimpleSetup(space) ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) start = ob.State(space) # we can pick a random start state... start.random() # ... or set specific values start().setX(.5) goal = ob.State(space) # we can pick a random goal state... goal.random() # ... or set specific values goal().setX(-.5) ss.setStartAndGoalStates(start, goal) # this will automatically choose a default planner with # default parameters solved = ss.solve(1.0) if solved: # try to shorten the path ss.simplifySolution() # print the simplified path print ss.getSolutionPath()
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 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 space_creation(self): """ Function of creating a configuration space """ space = ob.SE2StateSpace() bounds = ob.RealVectorBounds(2) bounds.setLow(const.LOW_BOUNDS) bounds.setHigh(const.HIGH_BOUNDS) space.setBounds(bounds) return space
def space_creation(self): """ Function of creating a configuration space """ space = ob.SE2StateSpace() bounds = ob.RealVectorBounds(2) bounds.setLow(self.low_bounds) bounds.setHigh(self.high_bounds) space.setBounds(bounds) return space
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 __init__(self, point_robot_manager): self.point_robot_manager = point_robot_manager # create an SE2 state space space = ob.SE2StateSpace() # set lower and upper bounds bounds = ob.RealVectorBounds(2) bounds.setLow(0, -point_robot_manager.dimension_length) bounds.setLow(1, -point_robot_manager.dimension_length) bounds.setHigh(0, point_robot_manager.dimension_length) bounds.setHigh(1, point_robot_manager.dimension_length) space.setBounds(bounds) self.space = space
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 __init__(self, setup=None, contact_resolution=0.02, pushing_dists=[0.02,0.04,0.06,0.08,0.10], debug=False, use_multiproc=False): self.name2cells = {} self.space = ob.SE2StateSpace() bounds = ob.RealVectorBounds(2) bounds.setLow(-1) bounds.setHigh(1) self.space.setBounds(bounds) self.si = ob.SpaceInformation(self.space) self.name_actor = 'finger' self.type_actor = 'finger' self.color_actor = [1,1,0,1] self.dims_actor = [0.03, 0.03] self.name2names = {} self.name2probs = {} self.sim_cols = {} self.sim_stbs = {} self.sim_objs = {} self.stbs_objs = {} self.trans_objs = {} self.actions_objs = {} self.n_actions_objs = {} self.uvec_contact2com = {} self.com_objs = {} if setup is None: setup = {'table_type','table_round'} self.setup = setup self.setup['table_radius'] = SimBullet.get_shapes()[self.setup['table_type']]['radius'] self.debug = debug if self.debug: self.sim_debug = SimBullet(gui=self.debug) self.addDefaultObjects(self.sim_debug) self.resolution_contact = contact_resolution self.pushing_dists = sorted(pushing_dists) self.use_multiproc = use_multiproc if self.use_multiproc: self.pool = Pool(int(multiprocessing.cpu_count()*0.8+0.5))
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 planTheHardWay(): # create an SE2 state space space = ob.SE2StateSpace() # set lower and upper bounds bounds = ob.RealVectorBounds(2) bounds.setLow(-1) bounds.setHigh(1) space.setBounds(bounds) # construct an instance of space information from this state space si = ob.SpaceInformation(space) # set state validity checking for this space si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) # create a random start state start = ob.State(space) start.random() # create a random goal state goal = ob.State(space) goal.random() # create a problem instance pdef = ob.ProblemDefinition(si) # set the start and goal states pdef.setStartAndGoalStates(start, goal) # create a planner for the defined space planner = og.RRTConnect(si) # set the problem we are trying to solve for the planner planner.setProblemDefinition(pdef) # perform setup steps for the planner planner.setup() # print the settings for this space print(si.settings()) # print the problem settings print(pdef) # attempt to solve the problem within one second of planning time solved = planner.solve(1.0) if solved: # get the goal representation from the problem definition (not the same as the goal state) # and inquire about the found path path = pdef.getSolutionPath() print("Found solution:\n%s" % path) else: print("No solution found")
def 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 plan(run_time, planner_type, wind_direction_degrees, dimensions, start_pos, goal_pos, obstacles, heading_degrees, state_sampler='', objective_type='sailing'): # Construct the robot state space in which we're planning space = ob.SE2StateSpace() # Create bounds on the position bounds = ob.RealVectorBounds(2) x_min, y_min, x_max, y_max = dimensions bounds.setLow(0, x_min) bounds.setLow(1, y_min) bounds.setHigh(0, x_max) bounds.setHigh(1, y_max) space.setBounds(bounds) # Use custom state sampler if len(state_sampler) > 0: if 'grid' in state_sampler.lower(): space.setStateSamplerAllocator( ob.StateSamplerAllocator(GridStateSampler)) else: print("WARNING: Unknown state_sampler = {}".format(state_sampler)) # Define a simple setup class ss = og.SimpleSetup(space) # Construct a space information instance for this state space si = ss.getSpaceInformation() # Set resolution of state validity checking, which is fraction of space's extent (Default is 0.01) desiredResolution = VALIDITY_CHECKING_RESOLUTION_KM / ss.getStateSpace( ).getMaximumExtent() si.setStateValidityCheckingResolution(desiredResolution) # Set the objects used to check which states in the space are valid validity_checker = ph.ValidityChecker(si, obstacles) ss.setStateValidityChecker(validity_checker) # Set our robot's starting state start = ob.State(space) start[0] = start_pos[0] start[1] = start_pos[1] # Set our robot's goal state goal = ob.State(space) goal[0] = goal_pos[0] goal[1] = goal_pos[1] # Set the start and goal states ss.setStartAndGoalStates(start, goal) # Create the optimization objective (helper function is simply a switch statement) objective = ph.allocate_objective(si, objective_type, wind_direction_degrees, heading_degrees) ss.setOptimizationObjective(objective) # Construct the optimal planner (helper function is simply a switch statement) optimizing_planner = allocatePlanner(si, planner_type) ss.setPlanner(optimizing_planner) # Attempt to solve the planning problem in the given runtime ss.solve(run_time) # Return the SimpleSetup object, which contains the solutionPath and spaceInformation # Must return ss, or else the object will be removed from memory return ss
boundsize = [0.0, 20.0] start_pt = [1.0, 19.0] goal_pt = [19.0, 1.0] def isStateValid(state): # print dir(state) # state.getX state.getY state.getYaw # state.setX state.setXY state.setY state.setYaw validity = True return validity #### Initiating Space space = ob.SE2StateSpace() #### Initiating Bounds bounds = ob.RealVectorBounds(2) bounds.setLow(boundsize[0]) bounds.setHigh(boundsize[1]) #### Setting Bounds to Space space.setBounds(bounds) #### Create Setup Object setup = og.SimpleSetup(space) setup.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) #### Initiating Start and Goal start = ob.State(space) start().setX(start_pt[0])
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
pos_cnstr = sqrt(x * x + y * y) return pos_cnstr > 0.2 def isStateValid_SE2(state): return boxConstraint(state.getX(), state.getY()) and state.getYaw() < pi / 2.0 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()
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
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) # define a simple setup class ss = og.SimpleSetup(space) # Construct a space information instance for this state space si = ss.getSpaceInformation() # Set resolution of state validity checking. This is fraction of space's extent. # si.setStateValidityCheckingResolution(0.001) # 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) lengthObj = ob.PathLengthOptimizationObjective(si) clearObj = ClearanceObjective(si) minTurnObj = MinTurningObjective(si) windObj = WindObjective(si) tackingObj = TackingObjective(si) ss.setOptimizationObjective(objective) print("ss.getProblemDefinition().hasOptimizationObjective( ){}".format(ss.getProblemDefinition().hasOptimizationObjective())) print("ss.getProblemDefinition().hasOptimizedSolution() {}".format(ss.getProblemDefinition().hasOptimizedSolution())) # 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) optimizing_planner = allocatePlanner(si, planner_type) 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)) solution_path = ss.getSolutionPath() states = solution_path.getStates() prevState = states[0] for state in states[1:]: print(space.validSegmentCount(prevState, state)) prevState = state print("ss.getSolutionPath().printAsMatrix() = {}".format(ss.getSolutionPath().printAsMatrix())) print("ss.haveSolutionPath() = {}".format(ss.haveSolutionPath())) print("ss.haveExactSolutionPath() = {}".format(ss.haveExactSolutionPath())) print("***") print("ss.getSolutionPath().length() = {}".format(ss.getSolutionPath().length())) print("ss.getSolutionPath().check() = {}".format(ss.getSolutionPath().check())) print("ss.getSolutionPath().clearance() = {}".format(ss.getSolutionPath().clearance())) print("ss.getSolutionPath().cost(objective).value() = {}".format(ss.getSolutionPath().cost(objective).value())) print("ss.getSolutionPath().cost(lengthObj).value() = {}".format(ss.getSolutionPath().cost(lengthObj).value())) print("ss.getSolutionPath().cost(clearObj).value() = {}".format(ss.getSolutionPath().cost(clearObj).value())) print("ss.getSolutionPath().cost(minTurnObj).value() = {}".format(ss.getSolutionPath().cost(minTurnObj).value())) print("ss.getSolutionPath().cost(windObj ).value() = {}".format(ss.getSolutionPath().cost(windObj).value())) print("ss.getSolutionPath().cost(tackingObj ).value() = {}".format(ss.getSolutionPath().cost(tackingObj).value())) print("ss.getProblemDefinition().hasOptimizedSolution() {}".format(ss.getProblemDefinition().hasOptimizedSolution())) plot_path(ss.getSolutionPath(), dimensions, obstacles) print("***") # print("Simplifying path") # ss.simplifySolution() # solution_path = ss.getSolutionPath() # states = solution_path.getStates() # prevState = states[0] # for state in states[1:]: # print(space.validSegmentCount(prevState, state)) # prevState = state # print("ss.getSolutionPath().printAsMatrix() = {}".format(ss.getSolutionPath().printAsMatrix())) # print("ss.haveSolutionPath() = {}".format(ss.haveSolutionPath())) # print("ss.haveExactSolutionPath() = {}".format(ss.haveExactSolutionPath())) # print("***") # print("ss.getSolutionPath().length() = {}".format(ss.getSolutionPath().length())) # print("ss.getSolutionPath().check() = {}".format(ss.getSolutionPath().check())) # print("ss.getSolutionPath().clearance() = {}".format(ss.getSolutionPath().clearance())) # print("ss.getSolutionPath().cost(objective).value() = {}".format(ss.getSolutionPath().cost(objective).value())) # print("ss.getSolutionPath().cost(lengthObj).value() = {}".format(ss.getSolutionPath().cost(lengthObj).value())) # print("ss.getSolutionPath().cost(clearObj).value() = {}".format(ss.getSolutionPath().cost(clearObj).value())) # print("ss.getSolutionPath().cost(minTurnObj).value() = {}".format(ss.getSolutionPath().cost(minTurnObj).value())) # plot_path(ss.getSolutionPath(), dimensions, obstacles) else: print("No solution found.")
def __init__(self, node_type, num_planners=1): """ Constructor for OMPLPlanTrajectoryWrapper. In orignal PathTools, it is using multi-threading with multiple ROS services/machines. Hence it required locks to record if each service/machine is idle or busy. However, in this version we don't need to worry about that, since the planning function does not utilize any ROS services. It is only calling the OMPL library code (if there is no service involved in OMPL). And all shared variables won't be in danger in the trajectory_planning function. Hence it is safe to call many trajectory planning jobs, without the need of using locks. However, for API safety and future extension (we may use neural network in this setting) we'll still use the locks. But it can be removed from this library. Args: node_type (string): The type of planner that this is being used by, generally "pfs" or "rr". num_planners (int): The number of planner nodes that are being used. """ PathTools.PlanTrajectoryWrapper.__init__(self, node_type, num_planners) ## add OMPL setting for different environments self.env_name = rospy.get_param('env_name') self.planner_name = 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(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 setup(problem): OMPL_INFORM("Robot type is: %s" % str(problem["robot.type"])) ompl_setup = eval("oa.%s()" % problem["robot.type"]) problem["is3D"] = isinstance(ompl_setup.getGeometricComponentStateSpace(), ob.SE3StateSpace) if str(ompl_setup.getAppType()) == "GEOMETRIC": problem["isGeometric"] = True else: problem["isGeometric"] = False ompl_setup.setEnvironmentMesh(str(problem['env_loc'])) ompl_setup.setRobotMesh(str(problem['robot_loc'])) if problem["is3D"]: # Set the dimensions of the bounding box bounds = ob.RealVectorBounds(3) bounds.low[0] = float(problem['volume.min.x']) bounds.low[1] = float(problem['volume.min.y']) bounds.low[2] = float(problem['volume.min.z']) bounds.high[0] = float(problem['volume.max.x']) bounds.high[1] = float(problem['volume.max.y']) bounds.high[2] = float(problem['volume.max.z']) ompl_setup.getGeometricComponentStateSpace().setBounds(bounds) space = ob.SE3StateSpace() # Set the start state start = ob.State(space) start().setXYZ(float(problem['start.x']), float(problem['start.y']), \ float(problem['start.z'])) # Set the start rotation start().rotation().x = float(problem['start.q.x']) start().rotation().y = float(problem['start.q.y']) start().rotation().z = float(problem['start.q.z']) start().rotation().w = float(problem['start.q.w']) # Set the goal state goal = ob.State(space) goal().setXYZ(float(problem['goal.x']), float(problem['goal.y']), float(problem['goal.z'])) # Set the goal rotation goal().rotation().x = float(problem['goal.q.x']) goal().rotation().y = float(problem['goal.q.y']) goal().rotation().z = float(problem['goal.q.z']) goal().rotation().w = float(problem['goal.q.w']) start = ompl_setup.getFullStateFromGeometricComponent(start) goal = ompl_setup.getFullStateFromGeometricComponent(goal) ompl_setup.setStartAndGoalStates(start, goal) else: # Set the dimensions of the bounding box bounds = ob.RealVectorBounds(2) bounds.low[0] = float(problem['volume.min.x']) bounds.low[1] = float(problem['volume.min.y']) bounds.high[0] = float(problem['volume.max.x']) bounds.high[1] = float(problem['volume.max.y']) ompl_setup.getGeometricComponentStateSpace().setBounds(bounds) space = ob.SE2StateSpace() start = ob.State(space) start().setX(float(problem['start.x'])) start().setY(float(problem['start.y'])) start().setYaw(float(problem['start.yaw'])) goal = ob.State(space) goal().setX(float(problem['goal.x'])) goal().setY(float(problem['goal.y'])) goal().setYaw(float(problem['goal.yaw'])) start = ompl_setup.getFullStateFromGeometricComponent(start) goal = ompl_setup.getFullStateFromGeometricComponent(goal) ompl_setup.setStartAndGoalStates(start, goal) return ompl_setup
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.")