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()))