def motionCost(self, s1, s2):
p, t = self.map.traversable((s1[0], s1[1], self.theta),
_a(s2),
frame='abs')
if p > 0:
r = -np.log(p)
else:
r = np.inf
return ob.Cost(float(r))
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 motionCost(self, s1, s2):
x1 = round(s1[1] * (self.costmap.shape[0] - 1))
y1 = round(s1[0] * (self.costmap.shape[1] - 1))
x2 = round(s2[1] * (self.costmap.shape[0] - 1))
y2 = round(s2[0] * (self.costmap.shape[1] - 1))
xx, yy = line(x1, y1, x2, y2)
cost = 0
for idx in range(len(xx) - 1):
cost = cost + self.costmap[xx[idx + 1]][yy[idx + 1]]
#cost = (cost/(len(xx)))
return ob.Cost(cost)
def motionCost(self, s1, s2):
#print('State 1:'+ str(s1[0]),str(s1[1]))
#print('State 2:'+ str(s2[0]),str(s2[1]))
x1 = round(s1[0] * (z.shape[0] - 1))
y1 = round(s1[1] * (z.shape[1] - 1))
x2 = round(s2[0] * (z.shape[0] - 1))
y2 = round(s2[1] * (z.shape[1] - 1))
xx, yy, val = line_aa(x1, y1, x2, y2)
#img = np.zeros((z.shape[0], z.shape[1]), dtype=np.uint8)
#img[xx, yy] = val * 100
#print(img)
cost = 0
for idx in range(len(xx) - 1):
cost = cost + z[xx[idx + 1]][yy[idx + 1]] * 0.01
return ob.Cost(cost)
def stateCost(self, s):
return ob.Cost(1 / self.si_.getStateValidityChecker().clearance(s))
def stateCost(self, s):
return ob.Cost(self._si.getStateValidityChecker().cost(s))
def solve(problem):
"""
Given an instance of the Problem class, containing the problem configuration
data, solves the motion planning problem and returns either the solution
path or a failure message.
"""
# Sets up the robot type related information
ompl_setup = setup(problem)
# Load the planner
space_info = ompl_setup.getSpaceInformation()
if problem['planner'].startswith('ompl.control.Syclop'):
decomposition = ompl_setup.allocDecomposition()
planner = eval('%s(space_info, decomposition)' % problem['planner'])
ompl_setup.setPlanner(planner)
else:
planner = eval("%s(space_info)" % problem['planner'])
ompl_setup.setPlanner(planner)
# Set the optimization objective
objectives = {'length': 'PathLengthOptimizationObjective', \
'max_min_clearance': 'MaximizeMinClearanceObjective', \
'mechanical_work': 'MechanicalWorkOptimizationObjective'}
objective = objectives[problem['objective']]
obj = eval('ob.%s(space_info)' % objective)
cost = ob.Cost(float(problem['objective.threshold']))
obj.setCostThreshold(cost)
ompl_setup.setOptimizationObjective(obj)
# Set each parameter that was configured by the user
for param in problem['planner_params']:
planner.params().setParam(str(param), str(problem['planner_params'][param]))
# Solve the problem
solution = {}
solved = ompl_setup.solve(float(problem['solve_time']))
# Check for validity
if solved:
if problem['isGeometric']:
path = ompl_setup.getSolutionPath()
initialValid = path.check()
if initialValid:
# If initially valid, attempt to simplify
ompl_setup.simplifySolution()
# Get the simplified path
simple_path = ompl_setup.getSolutionPath()
simplifyValid = simple_path.check()
if simplifyValid:
path = simple_path
else:
OMPL_ERROR("Simplified path was invalid.")
else:
OMPL_ERROR("Invalid solution path.")
else:
path = ompl_setup.getSolutionPath().asGeometric()
OMPL_INFORM("Path simplification skipped due to non rigid body.")
# Interpolate path
ns = int(100.0 * float(path.length()) / \
float(ompl_setup.getStateSpace().getMaximumExtent()))
if problem["isGeometric"] and len(path.getStates()) < ns:
path.interpolate(ns)
if len(path.getStates()) != ns:
OMPL_WARN("Interpolation produced " + str(len(path.getStates())) + \
" states instead of " + str(ns) + " states.")
solution = format_solution(path, True)
else:
solution = format_solution(None, False)
solution['name'] = str(problem['name'])
solution['planner'] = ompl_setup.getPlanner().getName()
solution['status'] = str(solved)
# Store the planner data
pd = ob.PlannerData(ompl_setup.getSpaceInformation())
ompl_setup.getPlannerData(pd)
explored_states = []
for i in range(0, pd.numVertices()):
coords = []
state = pd.getVertex(i).getState()
coords.append(state.getX())
coords.append(state.getY())
if problem["is3D"]:
coords.append(state.getZ())
else:
coords.append(0)
explored_states.insert(i, coords)
solution["explored_states"] = explored_states
return solution
def stateCost(self, s):
return ob.Cost(0)
def motionCost(self, s1, s2):
s1 = _a(s1)
_, t = self.map.traversable((s1[0], s1[1], self.theta),
_a(s2),
frame='abs')
return ob.Cost(float(t))
def objStateCost(state):
return ob.Cost(0)
def stateCost(self, s):
return ob.Cost(1 / (self.si_.getStateValidityChecker().clearance(s) +
sys.float_info.min))
def stateCost(self, s):
if (self.si_.getStateValidityChecker().clearance(s) == 0):
return sys.maxsize
return ob.Cost(1 / self.si_.getStateValidityChecker().clearance(s))
def motionCost(self, s1, s2):
return ob.Cost(1)
def getPathLengthObjective(si, length):
obj = ob.PathLengthOptimizationObjective(si)
obj.setCostThreshold(ob.Cost(length))
return obj
def __init__(self, si):
super(MyOptimizationObjective, self).__init__(si)
self.setCostThreshold(ob.Cost(10))
def getThresholdPathLengthObj(si):
obj = ob.PathLengthOptimizationObjective(si)
obj.setCostThreshold(ob.Cost(1.51))
return obj
def motionCost(self, s1, s2):
return ob.Cost(math.fabs(s2.getYaw() - s1.getYaw()))
