def getBalancedObjective1(si, m, theta=0, k=1000.0): o1 = ob.PathLengthOptimizationObjective(si) o2 = SurvivalObjective(si, m, theta) opt = ob.MultiOptimizationObjective(si) opt.addObjective(o1, 1.0) opt.addObjective(o2, k) return opt
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 getBalancedObjective1(si, k=1.0): o1 = ob.PathLengthOptimizationObjective(si) o2 = DurationObjective(si) opt = ob.MultiOptimizationObjective(si) opt.addObjective(o1, 1.0) opt.addObjective(o2, k) return opt
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 set_weight(self, w): objective = ob.MultiOptimizationObjective(self._si) objective.addObjective(OmplPlannerImpl.MapCostObjective(self._si), w[0]) objective.addObjective(ob.PathLengthOptimizationObjective(self._si), w[1]) self._pdef.setOptimizationObjective(objective)
def getBalancedObjective1(self, si): lengthObj = ob.PathLengthOptimizationObjective(si) clearObj = ClearanceObjective(si) opt = ob.MultiOptimizationObjective(si) opt.addObjective(lengthObj, 5.0) opt.addObjective(clearObj, 1.0) return opt
def getBalancedObjective1(self, si): lengthObj = ob.PathLengthOptimizationObjective(si) clearObj_pop = self.ClearanceObjective(si, self.costmap_pop) clearObj_met = self.ClearanceObjective(si, self.costmap_met) opt = ob.MultiOptimizationObjective(si) opt.addObjective(lengthObj, peso_l) opt.addObjective(clearObj_pop, peso_p) opt.addObjective(clearObj_met, peso_m) return opt
def getBalancedObjective1(si): lengthObj = ob.PathLengthOptimizationObjective(si) lengthObj.setCostThreshold(ob.Cost(1.5)) clearObj = ClearanceObjective(si) opt = ob.MultiOptimizationObjective(si) opt.addObjective(lengthObj, 1.0) opt.addObjective(clearObj, 1.0) return opt
def getSailingObjective(si, windDirectionDegrees, headingDegrees): lengthObj = ob.PathLengthOptimizationObjective(si) minTurnObj = MinTurningObjective(si, headingDegrees) windObj = WindObjective(si, windDirectionDegrees) opt = ob.MultiOptimizationObjective(si) opt.addObjective(minTurnObj, MIN_TURN_WEIGHT) opt.addObjective(windObj, WIND_WEIGHT) opt.addObjective(lengthObj, LENGTH_WEIGHT) # REMOVING TO SAVE COMPUTATION AND SEE IF IMPROVES. # clearObj = ClearanceObjective(si) # opt.addObjective(clearObj, CLEARANCE_WEIGHT) return opt
def getBalancedObjective(si): lengthObj = ob.PathLengthOptimizationObjective(si) clearObj = ClearanceObjective(si) minTurnObj = MinTurningObjective(si) windObj = WindObjective(si) tackingObj = TackingObjective(si) opt = ob.MultiOptimizationObjective(si) opt.addObjective(lengthObj, 1.0) opt.addObjective(clearObj, 80.0) opt.addObjective(minTurnObj, 0.0) opt.addObjective(windObj, 0.0) opt.addObjective(tackingObj, 0.0) # opt.setCostThreshold(ob.Cost(5)) return opt
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 get_threshold_path_length_objective(si): obj = ob.PathLengthOptimizationObjective(si) # obj.setCostThreshold(ob.Cost(8)) return obj
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
def __init__(self, map, pose, target, max_time, objective='duration', planner=og.RRTstar, threshold=0.9, tolerance=0.3, planner_options={'range': 0.45}): space = ob.RealVectorStateSpace(2) bounds = ob.RealVectorBounds(2) bounds.setLow(0) bounds.setHigh(map.size) space.setBounds(bounds) s = ob.State(space) t = ob.State(space) self.theta = pose[-1] for arg, state in zip([pose[:2], target], [s, t]): for i, x in enumerate(arg): state[i] = x ss = og.SimpleSetup(space) ss.setStartAndGoalStates(s, t, tolerance) si = ss.getSpaceInformation() ss.setStateValidityChecker( ob.StateValidityCheckerFn(partial(valid, si))) self.motion_val = EstimatedMotionValidator(si, map, threshold, theta=self.theta) si.setMotionValidator(self.motion_val) if objective == 'duration': self.do = DurationObjective(si, map, self.theta) elif objective == 'survival': self.do = SurvivalObjective(si, map, self.theta) elif objective == 'balanced': self.do = getBalancedObjective1(si, map, self.theta) else: # the objective is the euclidean path length self.do = ob.PathLengthOptimizationObjective(si) ss.setOptimizationObjective(self.do) # RRTstar BITstar BFMT RRTsharp p = planner(si) if 'range' in planner_options: try: p.setRange(planner_options.get('range')) except AttributeError: pass ss.setPlanner(p) ss.setup() self.ss = ss self.max_time = max_time self.map = map self.s = pose self.t = list(target) + [pose[-1]] self.planner_options = planner_options if planner == og.RRTstar: self.planner_name = 'RRT*' else: self.planner_name = '' self.threshold = threshold self.tolerance = tolerance self.comp_duration = 0 self.objective = objective
start = ob.State(space) start.random() start[0] = float(startend[0]) start[1] = float(startend[1]) start[2] = float(0) start[3] = float(0) # create a random goal state goal = ob.State(space) goal.random() goal[0] = float(startend[6]) goal[1] = float(startend[7]) goal[2] = float(0) goal[3] = float(0) # create a problem instance pdef = ob.ProblemDefinition(si) pdef.setOptimizationObjective(ob.PathLengthOptimizationObjective(si)) # set the start and goal states pdef.setStartAndGoalStates(start, goal) optimizingPlanner = allocatePlanner(si, 'fmtstar') # 限制采1000个点 optimizingPlanner.setNumSamples(2000) optimizingPlanner.setExtendedFMT(True) # optimizingPlanner.setFreeSpaceVolume(1) # optimizingPlanner.setRadiusMultiplier(5) # set the problem we are trying to solve for the planner optimizingPlanner.setProblemDefinition(pdef) # perform setup steps for the planner optimizingPlanner.setup() solved = optimizingPlanner.solve(60)
def getPathLengthObjective(si): return ob.PathLengthOptimizationObjective(si)
def getPathLengthObjWithCostToGo(si): obj = ob.PathLengthOptimizationObjective(si) obj.setCostToGoHeuristic(ob.CostToGoHeuristic(ob.goalRegionCostToGo)) return obj
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, 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
def getPathLengthObjective(si, length): obj = ob.PathLengthOptimizationObjective(si) obj.setCostThreshold(ob.Cost(length)) return obj
def getPathLengthObjWithCostToGo(si): obj = ob.PathLengthOptimizationObjective(si) obj.setCostToGoHeuristic(ob.CostToGoHeuristic(admiss_heuristic)) return obj
def get_path_length_objective(si): return ob.PathLengthOptimizationObjective(si)
def getThresholdPathLengthObj(si): obj = ob.PathLengthOptimizationObjective(si) obj.setCostThreshold(ob.Cost(1.51)) return obj
def get_path_length_obj_with_cost_to_go(si): obj = ob.PathLengthOptimizationObjective(si) obj.setCostToGoHeuristic(ob.CostToGoHeuristic(ob.goalRegionCostToGo)) return obj