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 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 __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 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 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 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 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 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