def makePlanner(space, start, goal): """Creates a MotionPlan object for the given space, start, and goal. Returns (planner,optimizing) where optimizing is True if the planner should continue be run after the first solution path has been found""" #This sets a Probabilistic Road Map (PRM) planner that connects #a random point to its 10 nearest neighbors. If knn is set to 0, #the points are connected as long as they lie #within distance 0.1 of one another # MotionPlan.setOptions(type="prm",knn=10,connectionThreshold=1) #This line sets a Rapidly-exploring Random Tree (RRT) planner that #repeatedly extends the tree toward a random point at maximum #distance 0.25. It uses the bidirectional=True option, which grows #trees from both the start and the goal #MotionPlan.setOptions(type="rrt",connectionThreshold=2.0,perturbationRadius=2.5,bidirectional=True) #MotionPlan.setOptions(type="sbl",connectionThreshold=5.0,gridResolution=1.0,perturbationRadius=1.5,bidirectional=True) optimizing = False #Optimizing planners. Make sure to uncomment optimizing = True below. #This sets the PRM algorithm with shortcutting # MotionPlan.setOptions(type="prm",knn=10,connectionThreshold=1.0,shortcut=True) #This sets the RRT* algorithm MotionPlan.setOptions(type="rrt*", connectionThreshold=2.0, perturbationRadius=2.5) #This sets a fast-marching method algorithm (Note: this does not work properly with rotations) #MotionPlan.setOptions(type="fmm*") #This sets a random-restart + shortcutting RRT # MotionPlan.setOptions(type="rrt",connectionThreshold=2.0,perturbationRadius=2.5,bidirectional=True,restart=True,shortcut=True,restartTermCond="{foundSolution:1,maxIters:1000}") optimizing = True #create the planner, and return it along with the termination criterion planner = MotionPlan(space) return planner, optimizing
def makePlanner(space, start, goal): """Creates a MotionPlan object for the given space, start, and goal. Returns (planner,optimizing) where optimizing is True if the planner should continue be run after the first solution path has been found""" #TODO: In lab3c, you should tune these parameters # #This sets a Probabilistic Road Map (PRM) planner that connects #a random point to its 10 nearest neighbors. If knn is set to 0, #the points are connected as long as they lie #within distance 0.1 of one another # MotionPlan.setOptions(type="prm",knn=10,connectionThreshold=0.1) #This line sets a Rapidly-exploring Random Tree (RRT) planner that #repeatedly extends the tree toward a random point at maximum #distance 0.25. It uses the bidirectional=True option, which grows #trees from both the start and the goal #MotionPlan.setOptions(type="rrt",connectionThreshold=0.1,perturbationRadius=0.25,bidirectional=True) optimizing = False #Optimizing planners, for use in Lab3C, part 2. Make sure to uncomment optimizing = True below. #This sets the PRM algorithm with shortcutting #MotionPlan.setOptions(type="prm",knn=10,connectionThreshold=0.1,shortcut=True) #This sets the RRT* algorithm #MotionPlan.setOptions(type="rrt*",connectionThreshold=0.1,perturbationRadius=0.25) #This sets a fast-marching method algorithm #MotionPlan.setOptions(type="fmm*") #This sets a random-restart + shortcutting RRT MotionPlan.setOptions(type="rrt",connectionThreshold=0.1,perturbationRadius=0.25,bidirectional=True,restart=True,shortcut=True) optimizing = True #create the planner and return it along with the termination criterion planner = MotionPlan(space) return planner,optimizing
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 __init__(self, space, start, goal, initial_points=1000): GLProgram.__init__(self) self.space = space #PRM planner MotionPlan.setOptions( type="prm*", knn=10, connectionThreshold=0.2 ) # Change type based on what planner you want to use self.optimizingPlanner = True self.planner = MotionPlan(space) self.start = start self.goal = goal self.planner.addMilestone(start) self.planner.addMilestone(goal) self.components = int(self.planner.getStats()['numComponents']) print(self.components) self.path = [] self.G = None
def init(): space, s, g = spawnFunc() if PLANNER_TYPE == 'prm_neighborhood': plan = MotionPlan(space, type='prm', connectionThreshold=0.1, ignoreConnectedComponents=True) elif PLANNER_TYPE == 'prm_knn': plan = MotionPlan(space, type='prm', knn=5, ignoreConnectedComponents=True) elif PLANNER_TYPE == 'prmstar': plan = MotionPlan(space, type='prm*') elif PLANNER_TYPE == 'rrtstar': plan = MotionPlan(space, type='rrt*', bidirectional=False) else: raise ValueError("Invalid PLANNER_TYPE") plan.setEndpoints(s, g)
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 feasible_plan(world, robot, qtarget): """Plans for some number of iterations from the robot's current configuration to configuration qtarget. Returns the first path found. Returns None if no path was found, otherwise returns the plan. """ t0 = time.time() moving_joints = [1, 2, 3, 4, 5, 6, 7] space = robotplanning.makeSpace(world=world, robot=robot, edgeCheckResolution=1e-2, movingSubset=moving_joints) plan = MotionPlan(space, type='prm') #TODO: maybe you should use planToConfig? numIters = 0 t1 = time.time() print("Planning time,", numIters, "iterations", t1 - t0) #to be nice to the C++ module, do this to free up memory plan.space.close() plan.close() return path
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 planTransfer(world, objectIndex, hand, shift): """Plan a transfer path for the robot given in world, which is currently holding the object indexed by objectIndex in the hand hand. The desired motion should translate the object by shift without rotating the object. """ globals = Globals(world) obj = world.rigidObject(objectIndex) cspace = TransferCSpace(globals, hand, obj) robot = world.robot(0) qmin, qmax = robot.getJointLimits() #get the start config q0 = robot.getConfig() q0arm = [q0[i] for i in hand.armIndices] if not cspace.feasible(q0arm): print "Warning, arm start configuration is infeasible" print "TODO: Complete 2.a to bypass this error" raw_input() cspace.close() return None #TODO: get the ungrasp config using an IK solver qungrasp = None qungrasparm = None print "TODO: Complete 2.b to find a feasible ungrasp config" raw_input() solver = hand.ikSolver(robot, vectorops.add(obj.getTransform()[1], shift), (0, 0, 1)) #plan the transfer path between q0arm and qungrasparm print "Planning transfer motion to ungrasp config..." MotionPlan.setOptions(connectionThreshold=5.0, perturbationRadius=0.5) planner = MotionPlan(cspace, 'sbl') planner.setEndpoints(q0arm, qungrasparm) #TODO: do the planning print "TODO: Complete 2.c to find a feasible transfer path" raw_input() cspace.close() #lift arm path to whole configuration space path path = [] for qarm in planner.getPath(): path.append(q0[:]) for qi, i in zip(qarm, hand.armIndices): path[-1][i] = qi qpostungrasp = hand.open(qungrasp, 1.0) return path + [qpostungrasp]
def testPlannerSuccessRate(N=100, duration=10, spawnFunc=lambda: kinkTest(0.0025, False)): import time import matplotlib.pyplot as plt finished = [] for run in range(N): space, s, g = spawnFunc() space.eps = 1e-3 if run == 0 and False: #show space space.drawObstaclesMPL(plt.axes()) plt.scatter([s[0], g[0]], [s[1], g[1]]) plt.show() plan = MotionPlan(space, type='prm', knn=5) plan.setEndpoints(s, g) t0 = time.time() finished.append(None) while time.time() - t0 < duration: plan.planMore(5) if plan.getPath(): finished[-1] = time.time() - t0 print("Found path with", len(plan.getPath()), "milestones in", time.time() - t0, "s") break if finished[-1] is None: print("Failed to find path in", duration, "s") import numpy as np finished = [v for v in finished if v != None] hist, edges = np.histogram(finished, 20, (0, duration)) print(hist, edges) hist = hist * 100 / N chist = np.cumsum(hist) plt.bar(edges[:-1], 100 - chist, duration / 20) plt.xlabel('Time (s)') plt.ylabel('% failed') plt.xlim(0, duration) plt.ylim(0, 100) plt.savefig('histogram.png') plt.show()
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 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 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]
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
def planTransit(world, objectIndex, hand): globals = Globals(world) cspace = TransitCSpace(globals, hand) obj = world.rigidObject(objectIndex) robot = world.robot(0) qmin, qmax = robot.getJointLimits() #get the start config q0 = robot.getConfig() q0arm = [q0[i] for i in hand.armIndices] if not cspace.feasible(q0arm): print "Warning, arm start configuration is infeasible" #get the pregrasp config -- TODO: what if the ik solver doesn't work? qpregrasp = None qpregrasparm = None solver = hand.ikSolver(robot, obj.getTransform()[1], [0, 0, 1]) print "Trying to find pregrasp config..." solver.setMaxIters(100) solver.setTolerance(1e-3) res = solver.solve() if res: qpregrasp = robot.getConfig() qpregrasparm = [qpregrasp[i] for i in hand.armIndices] if not cspace.feasible(qpregrasparm): print "Pregrasp config infeasible" cspace.close() return None if qpregrasp == None: print "Pregrasp solve failed" cspace.close() return None print "Planning transit motion to pregrasp config..." MotionPlan.setOptions(connectionThreshold=5.0, perturbationRadius=0.5) planner = MotionPlan(cspace, 'sbl') planner.setEndpoints(q0arm, qpregrasparm) iters = 0 step = 10 while planner.getPath() == None and iters < 1000: planner.planMore(step) iters += step cspace.close() if planner.getPath() == None: print "Failed finding transit path" return None print "Success, found path with", len(planner.getPath()), "milestones" #lift arm path to whole configuration space path path = [] for qarm in planner.getPath(): path.append(q0[:]) for qi, i in zip(qarm, hand.armIndices): path[-1][i] = qi #add a path to the grasp configuration return path + [hand.open(path[-1], 0)]
def planFree(world, hand, qtarget): """Plans a free-space motion for the robot's arm from the current configuration to the destination qtarget""" globals = Globals(world) cspace = TransitCSpace(globals, hand) robot = world.robot(0) qmin, qmax = robot.getJointLimits() #get the start/goal config q0 = robot.getConfig() q0arm = [q0[i] for i in hand.armIndices] qtargetarm = [qtarget[i] for i in hand.armIndices] if not cspace.feasible(q0arm): print "Warning, arm start configuration is infeasible" if not cspace.feasible(qtargetarm): print "Warning, arm goal configuration is infeasible" print "Planning transit motion to target config..." MotionPlan.setOptions(connectionThreshold=5.0, perturbationRadius=0.5) planner = MotionPlan(cspace, 'sbl') planner.setEndpoints(q0arm, qtargetarm) iters = 0 step = 10 while planner.getPath() == None and iters < 1000: planner.planMore(step) iters += step cspace.close() if planner.getPath() == None: print "Failed finding transit path" return None print "Success" #lift arm path to whole configuration space path path = [] for qarm in planner.getPath(): path.append(q0[:]) for qi, i in zip(qarm, hand.armIndices): path[-1][i] = qi return path
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)
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