class CSpaceObstacleSolver: def __init__(self, space, x_bound=1.0, y_bound=1.0, milestones=(0, 0), initial_points=4000): self.space = space #PRM planner MotionPlan.setOptions(type="prm", knn=10, connectionThreshold=0.05, ignoreConnectedComponents=True) self.optimizingPlanner = False # we first plan a bit without goals just to have a good skeleton self.initial_points = initial_points self.x_bound = x_bound self.y_bound = y_bound self.milestones = milestones self.times = 0 self.planner = MotionPlan(space) print('planning initial roadmap with {} points'.format(initial_points)) self.planner.planMore(self.initial_points) self.connected = False # we now add each of the chosen points as a milestone: self.G = self.planner.getRoadmap() self.start_milestones = len(self.G[0]) for milestone in self.milestones: self.planner.addMilestone(milestone) self.components = int(self.planner.getStats()['numComponents']) print(self.components) self.path = [] self.G = None def get_adjacency_matrix_from_milestones(self): while (self.components > 1): print("Planning 100...") self.planner.planMore(100) self.path = self.planner.getPath() self.G = self.planner.getRoadmap() self.components = int(self.planner.getStats()['numComponents']) print(self.components) print( 'PRM connecting all milestones found - computing adjacency matrix') pathDict = dict() self.adjacency_matrix = np.zeros(shape=(len(self.milestones), len(self.milestones))) self.adjacency_matrix[:, :] = np.inf for i, milestone1 in tqdm( enumerate( range(self.start_milestones, self.start_milestones + 1 + len(self.milestones)))): # print(self.G[0][milestone1]) for j, milestone2 in enumerate( range(milestone1 + 1, self.start_milestones + len(self.milestones))): j = j + i + 1 path = self.planner.getPath(milestone1, milestone2) cost = self.planner.pathCost(path) self.adjacency_matrix[i, j] = cost self.adjacency_matrix[j, i] = cost pathDict[i, j] = pathDict[j, i] = path # print((i,j),(j,i)) print('calculated all distances') for i in range(self.adjacency_matrix.shape[0]): self.adjacency_matrix[i, i] = 0 return self.adjacency_matrix, pathDict
class CSpaceObstacleSolver1: def __init__(self, space, milestones=(0, 0), initial_points=4000, steps=100): self.space = space #PRM planner MotionPlan.setOptions(type="prm", knn=10, connectionThreshold=1, ignoreConnectedComponents=True) self.optimizingPlanner = False # we first plan a bit without goals just to have a good skeleton self.initial_points = initial_points self.steps = steps self.milestones = milestones self.times = 0 self.planner = MotionPlan(space) for milestone in self.milestones: self.planner.addMilestone(milestone) self.components = int(self.planner.getStats()['numComponents']) print(self.components) print('planning initial roadmap with {} points'.format(initial_points)) self.planner.planMore(self.initial_points) self.connected = False # we now add each of the chosen points as a milestone: self.G = self.planner.getRoadmap() self.start_milestones = len(self.G[0]) self.path = [] self.milestone_2 = 1 self.G = None self.count = 0 self.connected_list = {0} self.total_milestones = set(list(range(len(self.milestones)))) def compute_actual_distance(self, origins, ends): # weights = [] weights = np.linalg.norm(ends - origins, axis=1) return weights def compute_real_pairwise_distances(self, G_list): G = nx.Graph() G.add_nodes_from(range(len(G_list[0]))) G.add_edges_from(G_list[1]) edges = np.array(G_list[1]) nodes = np.array(G_list[0]) # for config,node in zip(G_list[0],nodes): # G.nodes[node]['config'] = config origins = nodes[edges[:, 0]] ends = nodes[edges[:, 1]] weights = self.compute_actual_distance(origins, ends) for weight, edge in zip(weights, edges): G.edges[edge]['weight'] = weight print('actually calculating distances') self.G = G distances_array = [] for i in tqdm(range(len(self.milestones))): distances_dict = dict( nx.algorithms.shortest_path_length(G, source=i, weight='weight')) this_distance = [] for j in range(len(self.milestones)): this_distance.append(distances_dict[j]) distances_array.append(this_distance) distances = np.array(distances_array) # self.G = G return distances def get_adjacency_matrix_from_milestones(self): while (self.connected == False): if (self.space.fraction > 0.2): print("Planning {}... components = {}".format( self.steps, int(self.planner.getStats()['numComponents']))) else: print("Focused Planning {}... components = {}".format( self.steps, int(self.planner.getStats()['numComponents']))) self.planner.planMore(self.steps) milestone_1 = 0 remaining = self.total_milestones - self.connected_list G_list = self.planner.getRoadmap() G = nx.Graph() # self.space.set_remaining_milestones(remaining,G_list) G.add_nodes_from(range(len(G_list[0]))) G.add_edges_from(G_list[1]) elements_with_zero = nx.algorithms.node_connected_component(G, 0) self.connected_list = self.total_milestones.intersection( elements_with_zero) print('connected so far: ', len(self.connected_list)) # self.components = # for milestone in remaining: # # print(milestone_1,milestone) # if(not(self.planner.getPath(milestone_1,milestone) == None)): # self.connected_list.update([milestone]) if (self.connected_list == self.total_milestones): self.connected = True if (self.space.fraction < 0.2): self.steps = 20 print( 'PRM connecting all milestones found - computing adjacency matrix') pathDict = dict() self.adjacency_matrix = np.zeros(shape=(len(self.milestones), len(self.milestones))) # self.adjacency_matrix[:,:] = 0 # for i,milestone1 in tqdm(enumerate(range(0,1 + len(self.milestones)))): # # print(self.G[0][milestone1]) # for j,milestone2 in enumerate(range(milestone1+1,len(self.milestones))): # j = j+i + 1 # path = self.planner.getPath(milestone1,milestone2) # cost = self.planner.pathCost(path) # self.adjacency_matrix[i,j] = cost # self.adjacency_matrix[j,i] = cost # pathDict[i,j] = pathDict[j,i] = path # print((i,j),(j,i)) rm = self.planner.getRoadmap() self.adjacency_matrix = self.compute_real_pairwise_distances(rm) print('calculated all distances') return self.adjacency_matrix, rm
class CSpaceObstacleProgram(GLProgram): def __init__(self, space, start=(0.1, 0.5), goal=(0.9, 0.5), x_bound=1.0, y_bound=1.0, milestones=(0, 0), initial_points=1000): GLProgram.__init__(self) self.space = space #PRM planner MotionPlan.setOptions(type="prm", knn=10, connectionThreshold=1, ignoreConnectedComponents=True) self.optimizingPlanner = False # we first plan a bit without goals just to have a good skeleton self.initial_points = initial_points self.x_bound = x_bound self.y_bound = y_bound self.milestones = milestones self.times = 0 self.planner = MotionPlan(space) print('planning initial roadmap with {} points'.format(initial_points)) self.planner.planMore(self.initial_points) self.connected = False #FMM* planner #MotionPlan.setOptions(type="fmm*") #self.optimizingPlanner = True #RRT planner #MotionPlan.setOptions(type="rrt",perturbationRadius=0.25,bidirectional=True) #self.optimizingPlanner = False # RRT* planner # MotionPlan.setOptions(type="rrt*") # self.optimizingPlanner = True #random-restart RRT planner #MotionPlan.setOptions(type="rrt",perturbationRadius=0.25,bidirectional=True,shortcut=True,restart=True,restartTermCond="{foundSolution:1,maxIters:1000}") #self.optimizingPlanner = True #OMPL planners: #Tested to work fine with OMPL's prm, lazyprm, prm*, lazyprm*, rrt, rrt*, rrtconnect, lazyrrt, lbtrrt, sbl, bitstar. #Note that lbtrrt doesn't seem to continue after first iteration. #Note that stride, pdst, and fmt do not work properly... #MotionPlan.setOptions(type="ompl:rrt",suboptimalityFactor=0.1,knn=10,connectionThreshold=0.1) #self.optimizingPlanner = True # we then add start, goal and milestones self.start = start self.goal = goal # self.planner.setEndpoints(start,goal) # we now add each of the chosen points as a milestone: self.G = self.planner.getRoadmap() self.start_milestones = len(self.G[0]) print(self.start_milestones) for milestone in self.milestones: self.planner.addMilestone(milestone) self.components = int(self.planner.getStats()['numComponents']) print(self.components) self.G = self.planner.getRoadmap() self.end_milestones = len(self.G[0]) print(self.end_milestones) # self.planner.addMilestone(self.start) # self.planner.addMilestone(self.goal) self.path = [] self.G = None def keyboardfunc(self, key, x, y): if key == ' ': if ((self.optimizingPlanner or not self.path) or (self.components > 1)): print("Planning 1...") self.planner.planMore(1) self.path = self.planner.getPath() self.G = self.planner.getRoadmap() self.components = int(self.planner.getStats()['numComponents']) print(self.components) self.refresh() elif key == 'p': if ((self.optimizingPlanner or not self.path) or (self.components > 1)): print("Planning 100...") self.planner.planMore(1000) self.path = self.planner.getPath() self.G = self.planner.getRoadmap() self.components = int(self.planner.getStats()['numComponents']) print(self.components) self.paths = [] # for i in range(self.start_milestones,self.end_milestones): # for j in range(i,self.end_milestones): # print('getting paths') # self.paths.append( self.planner.getPath(i,j)) self.refresh() # elif key=='g': # adjacency_matrix = np.zeros(shape = (len(self.milestones),len(self.milestones))) # adjacency_matrix[:,:] = np.inf # if(self.components == 1): # for i,milestone1 in tqdm(enumerate(range(self.start_milestones+1,len(self.milestones)))): # for j,milestone2 in enumerate(range(milestone1+1,len(self.milestones))): # path = self.planner.getPath(milestone1,milestone2) # cost = self.planner.pathCost(path) # adjacency_matrix[i,j] = cost # adjacency_matrix[j,i] = cost # print('calculated all distances') # return adjacency_matrix def display(self): glMatrixMode(GL_PROJECTION) glLoadIdentity() glOrtho(0, self.x_bound, self.y_bound, 0, -1, 1) glMatrixMode(GL_MODELVIEW) glLoadIdentity() glDisable(GL_LIGHTING) self.space.drawObstaclesGL() if ((self.path) and (self.components == 1)): self.paths = [] for i in range(self.start_milestones, self.end_milestones): for j in range(i + 1, self.end_milestones): print('getting paths') self.paths.append(self.planner.getPath(i, j)) #draw path # glColor3f(0,1,0) # glBegin(GL_LINE_STRIP) self.colors = [] for i, path in enumerate(self.paths): if (len(self.colors) < i + 1): self.colors.append( [np.random.rand(), np.random.rand(), np.random.rand()]) glColor3f(*self.colors[i]) glBegin(GL_LINE_STRIP) for q in path: glVertex2f(q[0], q[1]) glEnd() # for path in self.paths: # for q in path: # self.space.drawRobotGL(q) for milestone in self.milestones: self.space.drawRobotGL(milestone) else: for milestone in self.milestones: self.space.drawRobotGL(milestone) pass if self.G: #draw graph V, E = self.G glEnable(GL_BLEND) glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) glColor4f(0, 0, 0, 0.5) glPointSize(3.0) glBegin(GL_POINTS) for v in V: glVertex2f(v[0], v[1]) glEnd() glColor4f(0.5, 0.5, 0.5, 0.5) glBegin(GL_LINES) for (i, j) in E: glVertex2f(V[i][0], V[i][1]) glVertex2f(V[j][0], V[j][1]) glEnd() glDisable(GL_BLEND)
class CSpaceObstacleProgram(GLProgram): def __init__(self, space, start=(0.1, 0.5), goal=(0.9, 0.5)): GLProgram.__init__(self) self.space = space #PRM planner MotionPlan.setOptions(type="prm", knn=10, connectionThreshold=0.1) self.optimizingPlanner = False #FMM* planner #MotionPlan.setOptions(type="fmm*") #self.optimizingPlanner = True #RRT planner #MotionPlan.setOptions(type="rrt",perturbationRadius=0.25,bidirectional=True) #self.optimizingPlanner = False #RRT* planner #MotionPlan.setOptions(type="rrt*") #self.optimizingPlanner = True #random-restart RRT planner #MotionPlan.setOptions(type="rrt",perturbationRadius=0.25,bidirectional=True,shortcut=True,restart=True,restartTermCond="{foundSolution:1,maxIters:1000}") #self.optimizingPlanner = True #OMPL planners: #Tested to work fine with OMPL's prm, lazyprm, prm*, lazyprm*, rrt, rrt*, rrtconnect, lazyrrt, lbtrrt, sbl, bitstar. #Note that lbtrrt doesn't seem to continue after first iteration. #Note that stride, pdst, and fmt do not work properly... #MotionPlan.setOptions(type="ompl:rrt",suboptimalityFactor=0.1,knn=10,connectionThreshold=0.1) #self.optimizingPlanner = True self.planner = MotionPlan(space) self.start = start self.goal = goal self.planner.setEndpoints(start, goal) self.path = [] self.G = None def keyboardfunc(self, key, x, y): if key == ' ': if self.optimizingPlanner or not self.path: print "Planning 1..." self.planner.planMore(1) self.path = self.planner.getPath() self.G = self.planner.getRoadmap() self.refresh() elif key == 'p': if self.optimizingPlanner or not self.path: print "Planning 100..." self.planner.planMore(100) self.path = self.planner.getPath() self.G = self.planner.getRoadmap() self.refresh() def display(self): glMatrixMode(GL_PROJECTION) glLoadIdentity() glOrtho(0, 1, 1, 0, -1, 1) glMatrixMode(GL_MODELVIEW) glLoadIdentity() glDisable(GL_LIGHTING) self.space.drawObstaclesGL() if self.path: #draw path glColor3f(0, 1, 0) glBegin(GL_LINE_STRIP) for q in self.path: glVertex2f(q[0], q[1]) glEnd() for q in self.path: self.space.drawRobotGL(q) else: self.space.drawRobotGL(self.start) self.space.drawRobotGL(self.goal) if self.G: #draw graph V, E = self.G glEnable(GL_BLEND) glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) glColor4f(0, 0, 0, 0.5) glPointSize(3.0) glBegin(GL_POINTS) for v in V: glVertex2f(v[0], v[1]) glEnd() glColor4f(0.5, 0.5, 0.5, 0.5) glBegin(GL_LINES) for (i, j) in E: glVertex2f(V[i][0], V[i][1]) glVertex2f(V[j][0], V[j][1]) glEnd() glDisable(GL_BLEND)
class CSpaceObstacleSolver: def __init__(self, space, milestones=(0, 0), initial_points=4000, steps=100): self.space = space #PRM planner MotionPlan.setOptions(type="prm", knn=20, connectionThreshold=0.5, ignoreConnectedComponents=True) self.optimizingPlanner = False # we first plan a bit without goals just to have a good skeleton self.initial_points = initial_points self.steps = steps self.milestones = milestones self.times = 0 self.planner = MotionPlan(space) for milestone in self.milestones: self.planner.addMilestone(milestone.tolist()) self.components = int(self.planner.getStats()['numComponents']) # print(self.components) print('planning initial roadmap with {} points'.format(initial_points)) self.planner.planMore(self.initial_points) self.connected = False # we now add each of the chosen points as a milestone: self.G = self.planner.getRoadmap() self.start_milestones = len(self.G[0]) self.path = [] self.milestone_2 = 1 self.G = None self.count = 0 self.connected_list = {0} self.total_milestones = set(list(range(len(self.milestones)))) def compute_actual_distance(self, origins, ends): weights = [] for origin, end in zip(origins, ends): interp = self.space.interp(origin, end) interp = interp[:, :self.space.robot.numLinks()] positions = [] for cfig in interp: self.space.robot.setConfig(cfig) positions.append(self.space.lamp.getTransform()[1]) positions = np.array(positions) distance = np.linalg.norm(np.diff(positions, axis=0), axis=1).sum() weights.append(distance) return weights def compute_real_pairwise_distances(self, G_list): G = nx.Graph() G.add_nodes_from(range(len(G_list[0]))) G.add_edges_from(G_list[1]) edges = np.array(G_list[1]) nodes = np.array(G_list[0]) origins = nodes[edges[:, 0], :self.space.robot.numLinks()] ends = nodes[edges[:, 1], :self.space.robot.numLinks()] weights = self.compute_actual_distance(origins, ends) for weight, edge in zip(weights, edges): G.edges[edge]['weight'] = weight distances_array = [] for i in tqdm(range(self.milestones.shape[0])): distances_dict = dict( nx.algorithms.shortest_path_length(G, source=i, weight='weight')) this_distance = [] for j in range(self.milestones.shape[0]): this_distance.append(distances_dict[j]) distances_array.append(this_distance) distances = np.array(distances_array) self.G = G return distances def get_adjacency_matrix_from_milestones(self): while (self.connected == False): if (self.space.fraction > 0.3): print("Planning {}... components = {}".format( self.steps, int(self.planner.getStats()['numComponents']))) else: print("Focused Planning {}... components = {}".format( self.steps, int(self.planner.getStats()['numComponents']))) self.planner.planMore(self.steps) milestone_1 = 0 remaining = self.total_milestones - self.connected_list G_list = self.planner.getRoadmap() if (len(G_list[0]) > 2500): raise UnreachablePointsError( 'There are points in the planner that are not feasible after 2500 samples!' ) G = nx.Graph() self.space.set_remaining_milestones(remaining, G_list) G.add_nodes_from(range(len(G_list[0]))) G.add_edges_from(G_list[1]) elements_with_zero = nx.algorithms.node_connected_component(G, 0) self.connected_list = self.total_milestones.intersection( elements_with_zero) print('Remaining to connect: {}'.format( len(self.milestones) - len(self.connected_list))) if (self.connected_list == self.total_milestones): self.connected = True if (self.space.fraction < 0.3): self.steps = 20 print( 'PRM connecting all milestones found - computing adjacency matrix') rm = self.planner.getRoadmap() self.adjacency_matrix = self.compute_real_pairwise_distances(rm) print('calculated all distances') return self.adjacency_matrix, self.G, rm[0]