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