def createPathWithGivenPositionsXY(self, xys): def createState(spaceInformation, xy): state = spaceInformation.allocState() state.setXY(xy[0], xy[1]) return state # Create dummy path and change private variables directly state = sbot.BoatState(globalWaypoint=latlon(0.2, 0.2), position=latlon(0, 0), globalWindDirectionDegrees=90, globalWindSpeedKmph=10, AISData=AISMsg(), headingDegrees=0, speedKmph=0) path = createPath(state, runtimeSeconds=1, numRuns=2) spaceInformation = path.getSpaceInformation() # Create acutal path actualPath = og.PathGeometric(spaceInformation) for xy in xys: actualPath.append(createState(spaceInformation, xy)) path.getOMPLPath()._solutionPath = actualPath path._latlons = path._getLatlonsFromOMPLPath(path._omplPath) return path
def steerTo(start, end, obc, IsInCollision, step_sz=DEFAULT_STEP): # test if there is a collision free path from start to end, with step size # given by step_sz, and with generic collision check function # here we assume start and end are tensors # return 0 if in coliision; 1 otherwise start_t = time.time() start_ompl = ob.State(setup.getSpaceInformation()) start_ompl().setX(start[0].item()) start_ompl().setY(start[1].item()) start_ompl().setZ(start[2].item()) angle = np.array( [start[6].item(), start[3].item(), start[4].item(), start[5].item()]) angle = QtoAxisAngle(angle) start_ompl().rotation().setAxisAngle(angle[0], angle[1], angle[2], angle[3]) end_ompl = ob.State(setup.getSpaceInformation()) end_ompl().setX(end[0].item()) end_ompl().setY(end[1].item()) end_ompl().setZ(end[2].item()) angle = np.array( [end[6].item(), end[3].item(), end[4].item(), end[5].item()]) angle = QtoAxisAngle(angle) end_ompl().rotation().setAxisAngle(angle[0], angle[1], angle[2], angle[3]) path_ompl = og.PathGeometric(si) path_ompl.append(start_ompl()) path_ompl.append(end_ompl()) return path_ompl.check()
def findBestSolution(validSolutions, invalidSolutions): # If no valid solutions found, use the best invalid one. Do not perform any path simplifying on invalid paths. if len(validSolutions) == 0: # Set the average distance between waypoints. Must be done before cost calculation and comparison for solution in invalidSolutions: setAverageDistanceBetweenWaypoints(solution.getSolutionPath()) bestSolution = min(invalidSolutions, key=lambda x: x.getSolutionPath().cost( x.getOptimizationObjective()).value()) bestSolutionPath = bestSolution.getSolutionPath() minCost = bestSolutionPath.cost( bestSolution.getOptimizationObjective()).value() else: # Set the average distance between waypoints. Must be done before cost calculation and comparison for solution in validSolutions: setAverageDistanceBetweenWaypoints(solution.getSolutionPath()) # Find path with minimum cost. Can be either simplified or unsimplified path. # Need to recheck that simplified paths are valid before using minCost = sys.maxsize bestSolution = None bestSolutionPath = None for solution in validSolutions: # Check unsimplified path unsimplifiedPath = og.PathGeometric(solution.getSolutionPath()) unsimplifiedCost = unsimplifiedPath.cost( solution.getOptimizationObjective()).value() if unsimplifiedCost < minCost: bestSolution = solution bestSolutionPath = unsimplifiedPath minCost = unsimplifiedCost return bestSolution, bestSolutionPath, minCost
def solve(self, ptc): pdef = self.getProblemDefinition() goal = pdef.getGoal() si = self.getSpaceInformation() pi = self.getPlannerInputStates() st = pi.nextStart() while st: self.states_.append(st) st = pi.nextStart() solution = None approxsol = 0 approxdif = 1e6 while not ptc(): rstate = si.allocState() # pick a random state in the state space self.sampler_.sampleUniform(rstate) # check motion if si.checkMotion(self.states_[-1], rstate): self.states_.append(rstate) sat = goal.isSatisfied(rstate) dist = goal.distanceGoal(rstate) if sat: approxdif = dist solution = len(self.states_) break if dist < approxdif: approxdif = dist approxsol = len(self.states_) solved = False approximate = False if not solution: solution = approxsol approximate = True if solution: path = og.PathGeometric(si) for s in self.states_[:solution]: path.append(s) pdef.addSolutionPath(path) solved = True return ob.PlannerStatus(solved, approximate)
def solve(self, ptc): pdef = self.getProblemDefinition() si = self.getSpaceInformation() pi = self.getPlannerInputStates() goal = pdef.getGoal() st = pi.nextStart() while st: self.states_.append(st) st = pi.nextStart() start_state = pdef.getStartState(0) goal_state = goal.getState() self.tree.append(Node(None, start_state)) solution = None approxsol = 0 approxdif = 1e6 while not ptc(): if self.expand(self.tree, goal_state) == GrowState.Reached: current = self.tree[-1] path = [] while current is not None: path.append(current.position) current = current.parent for i in range(1, len(path)): self.states_.append(path[len(path) - i - 1]) solution = len(self.states_) break solved = False approximate = False if not solution: solution = approxsol approximate = True if solution: path = og.PathGeometric(si) for s in self.states_[:solution]: path.append(s) pdef.addSolutionPath(path) solved = True return ob.PlannerStatus(solved, approximate)
def solve(self, ptc): pdef = self.getProblemDefinition() goal = pdef.getGoal() si = self.getSpaceInformation() pi = self.getPlannerInputStates() st = pi.nextStart() while st: self.states_.append(st) st = pi.nextStart() solution = None approxsol = 0 approxdif = 1e6 start_state = pdef.getStartState(0) goal_state = goal.getState() start_node = Node(None, start_state) start_node.g = start_state.h = start_node.f = 0 end_node = Node(None, goal_state) end_node.g = end_node.h = end_node.f = 0 open_list = [] closed_list = [] heapq.heapify(open_list) adjacent_squares = ((1, 0, 0), (1, 1, 45), (0, 1, 90), (-1, 1, 135), (-1, 0, 0), (-1, -1, -135), (0, -1, -90), (1, -1, -45)) heapq.heappush(open_list, start_node) while len(open_list) > 0 and not ptc(): current_node = heapq.heappop(open_list) if current_node == end_node: # if we hit the goal current = current_node path = [] while current is not None: path.append(current.position) current = current.parent for i in range(1, len(path)): self.states_.append(path[len(path) - i - 1]) solution = len(self.states_) break closed_list.append(current_node) children = [] for new_position in adjacent_squares: node_position = si.allocState() current_node_x = current_node.position.getX() current_node_y = current_node.position.getY() node_position.setXY(current_node_x + new_position[0], current_node_y + new_position[1]) node_position.setYaw(new_position[2] * math.pi / 180) if not si.checkMotion(current_node.position, node_position): continue if not si.satisfiesBounds(node_position): continue new_node = Node(current_node, node_position) children.append(new_node) for child in children: if child in closed_list: continue if child.position.getYaw() % (math.pi / 2) == 0: child.g = current_node.g + 1 else: child.g = current_node.g + math.sqrt(2) child.h = goal.distanceGoal(child.position) child.f = child.g + child.h if len([i for i in open_list if child == i and child.g >= i.g ]) > 0: continue heapq.heappush(open_list, child) solved = False approximate = False if not solution: solution = approxsol approximate = True if solution: path = og.PathGeometric(si) for s in self.states_[:solution]: path.append(s) pdef.addSolutionPath(path) solved = True return ob.PlannerStatus(solved, approximate)
def construct_path(si, states): path = og.PathGeometric(si) for s in states: path.append(s) return path
def solve(self, ptc): pdef = self.getProblemDefinition() si = self.getSpaceInformation() pi = self.getPlannerInputStates() goal = pdef.getGoal() st = pi.nextStart() while st: self.states_.append(st) st = pi.nextStart() start_state = pdef.getStartState(0) goal_state = goal.getState() self.treeA.append(Node(None, start_state)) self.treeB.append(Node(None, goal_state)) solution = None approxsol = 0 approxdif = 1e6 random_state = si.allocState() while not ptc(): # new random node self.sampler_.sampleUniform(random_state) if self.expand(self.treeA, random_state) != GrowState.Trapped: if self.connect(self.treeB, self.treeA[-1].position) == GrowState.Reached: if self.treeA[0].position == start_state: start_tree = self.treeA end_tree = self.treeB print('A to start') else: start_tree = self.treeB end_tree = self.treeA print('B to start') path_from_mid_to_start = [] path_from_mid_to_end = [] current = start_tree[-1] while current.position != start_state: path_from_mid_to_start.append(current.position) current = current.parent current = end_tree[-1] while current is not None: path_from_mid_to_end.append(current.position) current = current.parent path = path_from_mid_to_start[::-1] + path_from_mid_to_end for i in range(0, len(path)): self.states_.append(path[i]) solution = len(self.states_) #if self.treeB[0].position == start_state: # print('B to start') # path_from_mid_to_start = [] # path_from_mid_to_end = [] # current = self.treeB[-1] # while current.position != start_state: # path_from_mid_to_start.append(current.position) # current = current.parent # current = self.treeA[-1] # while current is not None: # path_from_mid_to_end.append(current.position) # current = current.parent # path = path_from_mid_to_start[::-1] + path_from_mid_to_end # for i in range(0, len(path)): # self.states_.append(path[i]) # solution = len(self.states_) break self.treeA, self.treeB = self.treeB, self.treeA # print('treeA:', self.treeA) # print('treeB:', self.treeB) solved = False approximate = False if not solution: solution = approxsol approximate = True if solution: path = og.PathGeometric(si) for s in self.states_[:solution]: path.append(s) pdef.addSolutionPath(path) solved = True return ob.PlannerStatus(solved, approximate)
def isValid(self, state): return bool(self.clearance(state) > 0.0) def clearance(self, state): x = state[0] y = state[1] return np.sqrt(x * x + y * y) - 1 space = ob.RealVectorStateSpace(2) bounds = ob.RealVectorBounds(2) bounds.setLow(-2.5) bounds.setHigh(2.5) space.setBounds(bounds) ss = og.SimpleSetup(space) si = ss.getSpaceInformation() ss.setStateValidityChecker(ValidityChecker(ss.getSpaceInformation())) state1 = space.allocState() si.allocStateSampler().sampleUniform(state1) state2 = space.allocState() si.allocStateSampler().sampleUniform(state2) path = og.PathGeometric(si) path.append(state1) path.append(state2) path.check()
def neural_plan_trajectory(self, start_point, goal_point, planner_number, joint_names, group_name, planning_time, planner_config_name, plan_type='pfs'): """ Given a start and goal point, plan by Neural Network. Args: start_point (list of float): A starting joint configuration. goal_point (list of float): A goal joint configuration. planner_number (int): The index of the planner to be used as returned by acquire_planner(). joint_names (list of str): The name of the joints corresponding to start_point and goal_point. group_name (str): The name of the group to which the joint names correspond. planning_time (float): Maximum allowed time for planning, in seconds. planner_config_name (str): Type of planner to use. Return: list of list of float: A sequence of points representing the joint configurations at each point on the path. """ """ # TODO: add hybrid planning for Baxter environment """ # obtain obstacle information through rostopic rospy.loginfo( "%s Plan Trajectory Wrapper: waiting for obstacle message..." % (rospy.get_name())) obc = rospy.wait_for_message('obstacles/obc', Float64Array2D) obc = torch.FloatTensor([obc_i.values for obc_i in obc.points]) obs = rospy.wait_for_message('obstacles/obs', Float64Array) obs = obs.values obs = torch.FloatTensor(obs) rospy.loginfo( "%s Plan Trajectory Wrapper: obstacle message received." % (rospy.get_name())) rospy.loginfo( '%s Plan Trajectory Wrapper: using neural network for planning...' % (rospy.get_name())) step_sz = DEFAULT_STEP MAX_NEURAL_REPLAN = 11 IsInCollision = self.IsInCollision path = [torch.FloatTensor(start_point), torch.FloatTensor(goal_point)] mpNet = self.neural_planners[0] time_flag = False fp = 0 plan_time = time.time() def isStateValid(state): return not IsInCollision(state, obc) si = ob.SpaceInformation(self.space) si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) si.setup() for t in xrange(MAX_NEURAL_REPLAN): # adaptive step size on replanning attempts if (t == 2): step_sz = 1.2 elif (t == 3): step_sz = 0.5 elif (t > 3): step_sz = 0.1 if time_flag: path, time_norm = plan_general.neural_replan(mpNet, path, obc, obs, IsInCollision, \ self.normalize_func, self.unnormalize_func, t==0, step_sz=step_sz, \ time_flag=time_flag, device=self.device) else: path = plan_general.neural_replan(mpNet, path, obc, obs, IsInCollision, \ self.normalize_func, self.unnormalize_func, t==0, step_sz=step_sz, \ time_flag=time_flag, device=self.device) time_norm = 0 # print lvc time print('Neural Planner: path length: %d' % (len(path))) lvc_start = time.time() path = plan_general.lvc(path, obc, IsInCollision, step_sz=step_sz) print('Neural Planner: lvc time: %f' % (time.time() - lvc_start)) feasible_check_time = time.time() # check feasibility using OMPL path_ompl = og.PathGeometric(si) for i in range(len(path)): state = ob.State(self.space) for j in range(len(path[i])): state[j] = path[i][j].item() sref = state() # a reference to the state path_ompl.append(sref) #path.check() if path_ompl.check(): #if plan_general.feasibility_check(path, obc, IsInCollision, step_sz=0.01): fp = 1 rospy.loginfo('%s Neural Planner: plan is feasible.' % (rospy.get_name())) break if self.finished: # if other planner has finished, stop break if time.time() - plan_time >= planning_time: # we can't allow the planner to go too long break if fp: # only for successful paths plan_time = time.time() - plan_time plan_time -= time_norm print('test time: %f' % (plan_time)) ## TODO: make sure path is indeed list return plan_time, path else: return np.inf, None