def __init__(self, shape, calc_time, resolution, distance): ou.setLogLevel(ou.LOG_WARN) self._calc_time = calc_time self._space = ob.RealVectorStateSpace(2) bounds = ob.RealVectorBounds(2) bounds.setLow(0.0) bounds.setHigh(0, shape[0] - 1) bounds.setHigh(1, shape[1] - 1) self._space.setBounds(bounds) self._si = ob.SpaceInformation(self._space) self._validity_checker = OmplPlannerImpl.ValidityChecker(self._si) self._si.setStateValidityChecker(self._validity_checker) self._si.setStateValidityCheckingResolution(resolution) self._si.setup() self._start = ob.State(self._space) self._goal = ob.State(self._space) self._pdef = ob.ProblemDefinition(self._si) objective = ob.MultiOptimizationObjective(self._si) objective.addObjective(OmplPlannerImpl.MapCostObjective(self._si), 0.5) # ## setting length objective length_objective = ob.PathLengthOptimizationObjective(self._si) objective.addObjective(length_objective, 0.1) # objective.setCostThreshold(1000.0) self._pdef.setOptimizationObjective(objective) self._planner = og.RRTstar(self._si) self._planner.setRange(distance) self._planner.setup()
def setup_ompl(self): print "set space" self.space = ob.RealVectorStateSpace(dim=self.space_dimension) bounds = ob.RealVectorBounds(self.space_dimension) print "set bounds" for i in xrange(self.space_dimension): bounds.setLow(i, self.joint_constraints[0]) bounds.setHigh(i, self.joint_constraints[1]) print "set bounds 2" self.space.setBounds(bounds) print "set si" self.si = ob.SpaceInformation(self.space) print "set motion validator" 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) print "set state validiy checker" self.si.setStateValidityChecker( ob.StateValidityCheckerFn(self.motion_validator.isValid)) print "set motion validator" self.si.setMotionValidator(self.motion_validator) print "si.setup" self.si.setup() self.problem_definition = ob.ProblemDefinition(self.si)
def __init__(self, lower_bound, upper_bound): """Initialize the object.""" # Create an R2 state space self.space = ob.RealVectorStateSpace(2) # Set lower and upper bounds bounds = ob.RealVectorBounds(2) bounds.setLow(lower_bound) bounds.setHigh(upper_bound) self.space.setBounds(bounds) # Create a new space information self.si = ob.SpaceInformation(self.space) # Set the state validity checker self.si.setStateValidityChecker( ob.StateValidityCheckerFn(self.is_state_valid)) self.si.setup() # Set the problem definition self.pdef = ob.ProblemDefinition(self.si) # Setup an optimizing planner with RRT* self.optimizingPlanner = og.RRTstar(self.si) # Empty obstacles list self.obstacles = []
def test_optimal(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) objective = ob.MultiOptimizationObjective(si) objective.addObjective(MapCostObjective(si), 0.9) objective.addObjective(ob.PathLengthOptimizationObjective(si), 0.1) pdef.setOptimizationObjective(objective) planner = og.RRTstar(si) planner.setProblemDefinition(pdef) planner.setup() status = planner.solve(calc_time) if pdef.hasExactSolution(): states = pdef.getSolutionPath().getStates() cost = pdef.getSolutionPath().cost(objective).value() states_np = np.array([(state[0], state[1]) for state in states]) print(states) print(cost) print(states_np)
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 space_info_creation(self, space): """ Function of creating space information that includes valid and invalid states """ space_info = ob.SpaceInformation(space) space_info.setStateValidityChecker( ob.StateValidityCheckerFn(self.isStateValid)) space_info.setup() return space_info
def space_info_creation(self, space, obstacle_list): """ Function of creating space information that includes valid and invalid states """ space_info = ob.SpaceInformation(space) isValidFn = ob.StateValidityCheckerFn(partial(self.isStateValid, obstacle_list)) space_info.setStateValidityChecker(isValidFn) #space_info.setStateValidityChecker(ob.StateValidityCheckerFn(self.isStateValid)) space_info.setup() return space_info
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 __init__(self, ppm_file): self.ppm_ = ou.PPM() self.ppm_.loadFile(ppm_file) self.space = ob.RealVectorStateSpace() self.space.addDimension(0.0, self.ppm_.getWidth()) self.space.addDimension(0.0, self.ppm_.getHeight()) self.maxWidth_ = self.ppm_.getWidth() - 1 self.maxHeight_ = self.ppm_.getHeight() - 1 self.si = ob.SpaceInformation(self.space) # set state validity checking for this space self.si.setStateValidityChecker(ob.StateValidityCheckerFn( partial(Plane2DEnvironment.isStateValid, self))) self.si.setup() self.si.setStateValidityCheckingResolution(1.0 / self.space.getMaximumExtent())
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 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 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 get_invalid_sections_for_paths(self, orig_paths, group_name): """ Returns the invalid sections for a set of paths. Args: orig_paths (list of paths): The paths to collision check, represnted by a list of individual joint configurations. group_name (str): The joint group for which the paths were created. Return: list of list of pairs of indicies, where each index in a pair is the start and end of an invalid section. """ # obtain obstacle information through rostopic rospy.loginfo( "Invalid Section Wrapper: waiting for obstacle message...") obc = rospy.wait_for_message('obstacles/obc', Float64Array2D) # obs = rospy.wait_for_message('obstacles/obs', Float64Array2D) obc = [obc_i.values for obc_i in obc.points] obc = np.array(obc) rospy.loginfo("Invalid Section Wrapper: obstacle message received.") # transform from orig_paths # for each path, check the invalid sections """ general idea: invalid section: 1. start and end should be not in collision, but intermediate nodes are all in collision. 2. start and end not in collision, no intermediate nodes, but not line collision free we assume that for all paths, the start and end are not in collision (valid planning problem, since projected) psuedo code: tracking = False while True: if not tracking: if is last node, then break line search (including end point) to next node: fail -> update start -> current tracking = True success -> move to next node else: collision free on current point: fail -> move to next node success -> end -> current save invalid section (start - end) tracking -> False """ inv_sec_paths = [] # set up OMPL env for line searching IsInCollision = self.IsInCollision def isStateValid(state): return not IsInCollision(state, obc) for orig_path in orig_paths: si = ob.SpaceInformation(self.space) si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) si.setup() # use motionValidator motionVal = ob.DiscreteMotionValidator(si) path = orig_path states = [] for i in range(len(path)): state = ob.State(self.space) for j in range(len(path[i])): state[j] = path[i][j] states.append(state) inv_sec_path = [] start_i = -1 end_i = -1 invalid_tracking = False point_i = 0 while point_i < len(orig_path): if not invalid_tracking: if point_i == len(orig_path) - 1: break # line search to the next node ind = motionVal.checkMotion(states[point_i](), states[point_i + 1]()) # also consider the endpoint ind = ind and isStateValid(states[point_i + 1]) if ind == False: start_i = point_i invalid_tracking = True # move to next point point_i += 1 else: # collision check on current point ind = isStateValid(path[point_i]) if ind == True: end_i = point_i inv_sec_path.append([start_i, end_i]) invalid_tracking = False else: point_i += 1 inv_sec_paths.append(inv_sec_path) return inv_sec_paths
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 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. self.space = ob.RealVectorStateSpace(2) # Set the bounds of space to be in [0,1]. self.space.setBounds(0, 400.0) # Construct a space information instance for this state space self.si = ob.SpaceInformation(self.space) # Set the object used to check which states in the space are valid validityChecker = ValidityChecker(self.si, self.mapIm) self.si.setStateValidityChecker(validityChecker) self.si.setup() self.start = ob.State(self.space) self.start[0] = self.startPose[0] self.start[1] = self.startPose[1] self.goal = ob.State(self.space) self.goal[0] = self.goalPose[0] self.goal[1] = self.goalPose[1] # Create a problem instance self.pdef = ob.ProblemDefinition(self.si) # Set the start and goal states self.pdef.setStartAndGoalStates(self.start, self.goal) # Create the optimization objective specified by our command-line argument. # This helper function is simply a switch statement. self.pdef.setOptimizationObjective( self.allocateObjective(self.si, objectiveType)) # Construct the optimal planner specified by our command line argument. # This helper function is simply a switch statement. self.optimizingPlanner = self.allocatePlanner(self.si, plannerType) # Set the problem instance for our planner to solve self.optimizingPlanner.setProblemDefinition(self.pdef) self.optimizingPlanner.setup() # attempt to solve the planning problem in the given runtime solved = self.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( self.optimizingPlanner.getName(), self.pdef.getSolutionPath().length(), self.pdef.getSolutionPath().cost( self.pdef.getOptimizationObjective()).value())) print self.pdef.getSolutionPath() # 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 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()
def plan_trajectory(self, start_point, goal_point, planner_number, joint_names, group_name, planning_time, planner_config_name, plan_type='pfs'): """ Use OMPL library for planning. Obtain obstacle information from rostopic for collision checking """ # obtain obstacle information through rostopic rospy.loginfo( "%s Plan Trajectory Wrapper: waiting for obstacle message..." % (rospy.get_name())) obc = rospy.wait_for_message('obstacles/obc', Float64Array2D) # obs = rospy.wait_for_message('obstacles/obs', Float64Array2D) obc = [obc_i.values for obc_i in obc.points] obc = np.array(obc) rospy.loginfo( "%s Plan Trajectory Wrapper: obstacle message received." % (rospy.get_name())) # depending on plan type, obtain path_length from published topic or not if plan_type == 'pfs': # obtain path length through rostopic rospy.loginfo( "%s Plan Trajectory Wrapper: waiting for planning path length message..." % (rospy.get_name())) path_length = rospy.wait_for_message( 'planning/path_length_threshold', Float64) path_length = path_length.data rospy.loginfo( "%s Plan Trajectory Wrapper: planning path length received." % (rospy.get_name())) elif plan_type == 'rr': path_length = np.inf # set a very large path length because we only want feasible paths # reshape # plan IsInCollision = self.IsInCollision rospy.loginfo("%s Plan Trajectory Wrapper: start planning..." % (rospy.get_name())) # create a simple setup object start = ob.State(self.space) # we can pick a random start state... # ... or set specific values for k in xrange(len(start_point)): start[k] = start_point[k] goal = ob.State(self.space) for k in xrange(len(goal_point)): goal[k] = goal_point[k] def isStateValid(state): return not IsInCollision(state, obc) si = ob.SpaceInformation(self.space) si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) si.setup() pdef = ob.ProblemDefinition(si) pdef.setStartAndGoalStates(start, goal) pdef.setOptimizationObjective(getPathLengthObjective(si, path_length)) ss = allocatePlanner(si, self.planner_name) ss.setProblemDefinition(pdef) ss.setup() plan_time = time.time() solved = ss.solve(planning_time) plan_time = time.time() - plan_time if solved: rospy.loginfo( "%s Plan Trajectory Wrapper: OMPL Planner solved successfully." % (rospy.get_name())) # obtain planned path ompl_path = pdef.getSolutionPath().getStates() solutions = np.zeros((len(ompl_path), 2)) for k in xrange(len(ompl_path)): solutions[k][0] = float(ompl_path[k][0]) solutions[k][1] = float(ompl_path[k][1]) return plan_time, solutions.tolist() else: return np.inf, None
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 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.")
def shortcut_path(self, original_path, group_name): """ Shortcuts a path, where the path is for a given group name. Args: original_path (list of list of float): The path, represented by a list of individual joint configurations. group_name (str): The group for which the path was created. Return: list of list of float: The shortcutted version of the path. """ # obtain obstacle information through rostopic rospy.loginfo("Shortcut Path Wrapper: waiting for obstacle message...") obc = rospy.wait_for_message('obstacles/obc', Float64Array2D) # obs = rospy.wait_for_message('obstacles/obs', Float64Array2D) obc = [obc_i.values for obc_i in obc.points] obc = np.array(obc) #original_path = np.array(original_path) #print(original_path) #rospy.loginfo("Shortcut Path Wrapper: obstacle message received.") #path = plan_general.lvc(original_path, obc, self.IsInCollision, step_sz=rospy.get_param("step_size")) #path = np.array(path).tolist() # try using OMPL method for shortcutting IsInCollision = self.IsInCollision def isStateValid(state): return not IsInCollision(state, obc) si = ob.SpaceInformation(self.space) si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) si.setup() # use motionValidator motionVal = ob.DiscreteMotionValidator(si) path = original_path states = [] for i in range(len(path)): state = ob.State(self.space) for j in range(len(path[i])): state[j] = path[i][j] states.append(state) state_idx = list(range(len(states))) def lvc(path, state_idx): # state idx: map from path idx -> state idx for i in range(0, len(path) - 1): for j in range(len(path) - 1, i + 1, -1): ind = 0 ind = motionVal.checkMotion(states[state_idx[i]](), states[state_idx[j]]()) if ind == True: pc = [] new_state_idx = [] for k in range(0, i + 1): pc.append(path[k]) new_state_idx.append(state_idx[k]) for k in range(j, len(path)): pc.append(path[k]) new_state_idx.append(state_idx[k]) return lvc(pc, new_state_idx) return path path = lvc(original_path, state_idx) return path """ pathSimplifier = og.PathSimplifier(si) rospy.loginfo("Shortcut Path Wrapper: obstacle message received.") path = original_path path_ompl = og.PathGeometric(si) for i in range(len(path)): state = ob.State(self.space) for j in range(len(path[i])): state[j] = path[i][j] sref = state() # a reference to the state path_ompl.append(sref) # simplify by LVC path_ompl_states = path_ompl.getStates() solutions = np.zeros((len(path_ompl_states), len(path[0]))) pathSimplifier.collapseCloseVertices(path_ompl) for i in xrange(path_ompl.getStateCount()): for j in xrange(len(path[0])): solutions[i][j] = float(path_ompl.getState(i)[j]) """ return solutions
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) # set state validity checking for this space obs_manager = obs2fcl(obs, dimW) mychecker = ValidityChecker(si, obs_manager, space) si.setStateValidityChecker(mychecker) si.setStateValidityCheckingResolution(0.03) si.getStateSpace().setStateSamplerAllocator(ob.StateSamplerAllocator(allocMyStateSampler)) si.setup() # 起点终点 start = ob.State(space) start.random() start[0] = float(startend[0]) start[1] = float(startend[1]) start[2] = float(0) start[3] = float(0)
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 __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)
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() si_vec.append(si_R2) si_vec.append(si_SE2) # Define Planning Problem start_SE2 = ob.State(SE2)
def plan(self, controlled_object, x1, y1, v1, a1): a1 = (a1 + 2 * np.pi) % (2 * np.pi) car = deepcopy(controlled_object) space = ob.RealVectorStateSpace(3) bounds = ob.RealVectorBounds(3) bounds.setLow(0, 0) bounds.setLow(1, 0) bounds.setLow(2, 0) bounds.setHigh(0, 1000) bounds.setHigh(1, 1000) bounds.setHigh(2, 2 * np.pi) space.setBounds(bounds) si = ob.SpaceInformation(space) validityChecker = ValidityChecker(si, self.state, car) si.setStateValidityChecker(validityChecker) #si.setMotionValidator(MotionValidityChecker(si, state, agent_num)) si.setup() start = ob.State(space) start[0] = car.x start[1] = car.y start[2] = car.angle goal = ob.State(space) goal[0] = x1 goal[1] = y1 goal[2] = a1 pdef = ob.ProblemDefinition(si) pdef.setStartAndGoalStates(start, goal) pdef.setOptimizationObjective(ob.PathLengthOptimizationObjective(si)) def objStateCost(state): return ob.Cost(0) def objMotionCost(state1, state2): x, y, a = state1[0], state1[1], state1[2] xt, yt, at = state2[0], state2[1], state2[2] yd, xd = yt - y, xt - x ax = np.arctan(yd / (xd + 0.001)) ax = ax % (np.pi) if yd < 0: ax = ax + np.pi ad = min(np.abs(at - ax), np.abs((ax - at))) return 10 * ad pathObj = ob.OptimizationObjective(si) pathObj.stateCost = objStateCost pathObj.motionCost = objMotionCost pathObj.setCostThreshold(1) #pdef.setOptimizationObjective(pathObj) optimizingPlanner = og.RRTstar(si) optimizingPlanner.setRange(self.inter_point_d) optimizingPlanner.setProblemDefinition(pdef) optimizingPlanner.setup() solved = optimizingPlanner.solve(self.planning_time) sol = pdef.getSolutionPath() if not sol: return [] sol = sol.printAsMatrix() s = [[float(j) for j in i.split(" ")[:-1]] for i in sol.splitlines()][:-1] v0 = car.vel num_points = len(s) for i in range(num_points): [i].append(v0 * (1 - float(i) / float(num_points)) + v1 * (float(i) / float(num_points))) return s
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(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 plan(self, start_tup, goal_tup, turning_radius=10, planning_time=30.0): def isStateValid(state): y = int(state.getY()) x = int(state.getX()) if y < 0 or x < 0 or y >= self.map_data.shape[ 0] or x >= self.map_data.shape[1]: return False return bool(self.map_data[y, x] == 0) space = ob.DubinsStateSpace(turningRadius=turning_radius) # Set State Space bounds bounds = ob.RealVectorBounds(2) bounds.setLow(0, 0) bounds.setHigh(0, self.map_data.shape[1]) bounds.setLow(1, 0) bounds.setHigh(1, self.map_data.shape[0]) space.setBounds(bounds) si = ob.SpaceInformation(space) si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) si.setStateValidityCheckingResolution( self.params["validity_resolution"] ) # Set based on thinness of walls in map si.setValidStateSamplerAllocator( ob.ValidStateSamplerAllocator( ob.ObstacleBasedValidStateSampler(si))) si.setup() # Set Start and Goal states start = ob.State(space) start().setX(start_tup[0]) start().setY(start_tup[1]) start().setYaw(start_tup[2]) goal = ob.State(space) goal().setX(goal_tup[0]) goal().setY(goal_tup[1]) goal().setYaw(goal_tup[2]) # Set Problem Definition pdef = ob.ProblemDefinition(si) pdef.setStartAndGoalStates(start, goal) optimObj = ob.PathLengthOptimizationObjective(si) pdef.setOptimizationObjective(optimObj) # Set up planner optimizingPlanner = og.BITstar(si) optimizingPlanner.setProblemDefinition(pdef) optimizingPlanner.setup() solved = optimizingPlanner.solve(planning_time) def solution_path_to_tup(solution_path): result = [] states = solution_path.getStates() for state in states: x = state.getX() y = state.getY() yaw = state.getYaw() result.append((x, y, yaw)) return result solutionPath = None if solved: solutionPath = pdef.getSolutionPath() solutionPath.interpolate(self.params['interpolation_density']) solutionPath = solution_path_to_tup(solutionPath) return bool(solved), solutionPath