def animate_trajectories(trajs, times, endWaitTime=5.0, speed=0.2): global world active = 0 for i in range(len(trajs)): vis.add( "traj" + str(i), trajs[i].getLinkTrajectory( END_EFFECTOR_LINK, 0.1).getPositionTrajectory(END_EFFECTOR_LOCAL_POSITION)) vis.hide("traj" + str(i)) vis.hideLabel("traj" + str(i)) vis.show() t0 = time.time() if len(times) == 1: tnext = float('inf') else: tnext = times[active + 1] while vis.shown(): t = time.time() if t - t0 > tnext: nextactive = (active + 1) % len(times) vis.hide("traj" + str(active)) vis.hide("traj" + str(nextactive), False) active = nextactive if nextactive + 1 == len(times): tnext += endWaitTime else: tnext += times[active + 1] - times[active] vis.lock() world.robot(0).setConfig(trajs[active].eval((t - t0) * speed, endBehavior='loop')) vis.unlock() time.sleep(0.01) print("Done.")
def sphere_grid_test(N=5, staggered=True): from klampt import vis from klampt.model import trajectory if staggered: G = sphere_staggered_grid(N) else: G = sphere_grid(N) for n in G.nodes(): x = G.node[n]['params'] vis.add(str(n), x) vis.hideLabel(str(n)) #draw edges? minDist = float('inf') maxDist = 0.0 for i, j in G.edges(): xi = G.node[i]['params'] xj = G.node[j]['params'] vis.add(str(i) + '-' + str(j), trajectory.Trajectory([0, 1], [xi, xj])) vis.hideLabel(str(i) + '-' + str(j)) dist = vectorops.distance(xi, xj) if dist > maxDist: maxDist = dist print "Max dispersion at", i, j, ":", dist if dist < minDist: minDist = dist print "Number of points:", G.number_of_nodes() print "Min/max dispersion:", minDist, maxDist vis.run()
def show_friction_cone(folder="FrictionCones/StanfordMicroSpineUnit"): V = Volume() V.load(folder) for C in V.convex_decomposition: print "Convex decomposition" print np.amin(C.points,axis=0) print np.amax(C.points,axis=0) Chull = np.vstack([V.convex_hull.points[u] for u in V.convex_hull.vertices]) #print Chull Chull *= 0.05 for i,C in enumerate(V.convex_decomposition): if len(V.convex_decomposition) == 1: g = 0 else: g = float(i)/(len(V.convex_decomposition)-1) #for j,v in enumerate(C.vertices): # vis.add("pt%d,%d"%(i,j),vectorops.mul(C.points[v,:].tolist(),0.05)) # vis.setColor("pt%d,%d"%(i,j),1,g,0) # vis.hideLabel("pt%d,%d"%(i,j)) for j,pt in enumerate(C.points): v = j vis.add("pt%d,%d"%(i,j),vectorops.mul(C.points[v,:].tolist(),0.05)) vis.setColor("pt%d,%d"%(i,j),1,g,0) vis.hideLabel("pt%d,%d"%(i,j)) for i in xrange(Chull.shape[0]): vis.add("CH%d"%(i,),Chull[i,:].tolist()) vis.hideLabel("CH%d"%(i,)) vis.add("origin",(0,0,0)) vis.setColor("origin",0,1,0) vis.dialog()
def visualize_cost(x, y, cost, name, with_z=0, hm=None, color=None, hide_label=False): traj = trajectory.Trajectory(milestones=[[x, y, with_z], [x, y, with_z + cost]]) vis.add(name, traj) if color: VisUtils.set_color(name, color) if hide_label: vis.hideLabel(name)
def RobotCOMPlot(SimRobot, vis): COMPos_start = SimRobot.getCom() COMPos_end = COMPos_start[:] COMPos_end[2] = COMPos_end[2] - 7.50 vis.add("COM", Trajectory([0, 1], [COMPos_start, COMPos_end])) vis.hideLabel("COM", True) vis.setColor("COM", 0.0, 204.0 / 255.0, 0.0, 1.0) vis.setAttribute("COM", 'width', 5.0)
def COM2IntersectionPlot(i, vis, COM_Pos, Intersection): # This function is used to plot PIP from COM to intersections Edge_Index = str(i) vis.add("PIPEdgefromCOM:" + Edge_Index, Trajectory([0, 1], [COM_Pos, Intersection])) vis.hideLabel("PIPEdgefromCOM:" + Edge_Index, True) vis.setAttribute("PIPEdgefromCOM:" + Edge_Index, 'width', 7.5) vis.setColor("PIPEdgefromCOM:" + Edge_Index, 65.0 / 255.0, 199.0 / 255.0, 244.0 / 255.0, 1.0)
def ConvexEdgesPlot(SimRobot, convex_edges_list, vis): Convex_Edges_Number = len(convex_edges_list) / 2 COMPos = SimRobot.getCom() for i in range(0, Convex_Edges_Number): EdgeA = convex_edges_list[2 * i] EdgeB = convex_edges_list[2 * i + 1] # Three edges to be added: A->B, A -> COM, B-> COM Edge_Index = str(i) vis.add("Edge:" + Edge_Index, Trajectory([0, 1], [EdgeA, EdgeB])) vis.hideLabel("Edge:" + Edge_Index, True) vis.setAttribute("Edge:" + Edge_Index, 'width', 5.0)
def visualize_in_klampt(self, height_map, step=10): for x_inx in range(0, self.x_indices - 1, step): for y_inx in range(0, self.y_indices - 1, step): x_world = self.get_x_from_xindex(x_inx) y_world = self.get_y_from_yindex(y_inx) cost = self.np_cmap_arr[x_inx][y_inx] name = str(x_world) + "," + str(y_world) if cost > 0.001: base_z = height_map.height_at_xy(x_world, y_world) traj = trajectory.Trajectory( milestones=[[x_world, y_world, base_z + 0.001], [x_world, y_world, base_z + cost]]) vis.add(name, traj) vis.hideLabel(name)
def TransitionDataPlot(vis, StepNo, LimbNo, ReachableContacts_data): RowNo, ColumnNo = ReachableContacts_data.shape RowStart = 0 RowEnd = RowNo Traj = [] for i in range(RowStart, RowEnd): point_i = [0.0, 0.0, 0.0] ReachableContact_i = ReachableContacts_data[i] point_i[0] = ReachableContact_i[0] point_i[1] = ReachableContact_i[1] point_i[2] = ReachableContact_i[2] Traj.append(point_i) TransName = "Stage" + str(StepNo) + "LinkNo" + str(LimbNo) + "Path" vis.add(TransName, Trajectory([0, 1], Traj)) vis.hideLabel(TransName, True) vis.setColor(TransName, 255.0 / 255.0, 255.0 / 255.0, 51.0 / 255.0, 1.0)
def TransitionDataPlot(vis, StepNo, LimbNo, ReachableContacts_data): RowNo, ColumnNo = ReachableContacts_data.shape RowStart = 0 RowEnd = RowNo for i in range(RowStart, RowEnd): point_start = [0.0, 0.0, 0.0] ReachableContact_i = ReachableContacts_data[i] point_start[0] = ReachableContact_i[0] point_start[1] = ReachableContact_i[1] point_start[2] = ReachableContact_i[2] TransName = "Step" + str(StepNo) + "Limb" + str( LimbNo) + "TransPoint:" + str(i) vis.add(TransName, point_start) vis.hideLabel(TransName, True) vis.setColor(TransName, 255.0 / 255.0, 255.0 / 255.0, 51.0 / 255.0, 1.0)
def ContactDataPlot(vis, StepNo, LimbNo, ReachableContacts_data): RowNo, ColumnNo = ReachableContacts_data.shape RowStart = 0 RowEnd = RowNo for i in range(RowStart, RowEnd): point_start = [0.0, 0.0, 0.0] ReachableContact_i = ReachableContacts_data[i] point_start[0] = ReachableContact_i[0] point_start[1] = ReachableContact_i[1] point_start[2] = ReachableContact_i[2] ContactName = "Stage" + str(StepNo) + "LinkNo" + str( LimbNo) + "Point:" + str(i) vis.add(ContactName, point_start) vis.hideLabel(ContactName, True) vis.setColor(ContactName, 65.0 / 255.0, 199.0 / 255.0, 244.0 / 255.0, 1.0)
def ContactDataPlot(vis, ReachableContacts_data): RowNo, ColumnNo = ReachableContacts_data.shape RowStart = 0 # RowEnd = min(RowNo, 99) RowEnd = RowNo for i in range(RowStart, RowEnd): point_start = [0.0, 0.0, 0.0] ReachableContact_i = ReachableContacts_data[i] point_start[0] = ReachableContact_i[0] point_start[1] = ReachableContact_i[1] point_start[2] = ReachableContact_i[2] vis.add("Point_" + str(i), point_start) vis.hideLabel("Point_" + str(i), True) vis.setColor("Point_" + str(i), 65.0 / 255.0, 199.0 / 255.0, 244.0 / 255.0, 1.0)
def visualize_in_klampt(self, step=5, xmin=4, xmax=12, ymin=8, ymax=12): """Visualize the height map using klampt""" for x_inx in range(self.get_xindex_from_x(xmin), self.get_xindex_from_x(xmax), step): for y_inx in range(self.get_yindex_from_y(ymin), self.get_yindex_from_y(ymax), step): x_world = self.get_x_from_xindex(x_inx) y_world = self.get_y_from_yindex(y_inx) z = self.np_arr[x_inx][y_inx] if z > 0: name = str(x_world) + ", " + str(y_world) traj = trajectory.Trajectory( milestones=[[x_world, y_world, 0], [x_world, y_world, z]]) vis.add(name, traj) vis.hideLabel(name)
def draw_node(self, pos, colour=(1, 0, 0, 1), size=15, label=None): """Draw a node in the klampt visualiser :param pos: the position of the node :param colour: the colour of the node :param size: the size of the node :param label: if given, add label to the node """ from klampt import vis from klampt.model.coordinates import Point if label is None: label = repr(pos) vis.add(label, Point(pos), keepAppearance=True) vis.setAttribute(label, "size", size) vis.setAttribute(label, "color", colour) vis.hideLabel(label)
def PIP_Subplot(i, EdgeA, EdgeB, EdgeCOM, Edgex, Edgey, Edgez, COM_Pos, vis): scale = 0.25 Edge_Index = str(i) vis.add("PIPEdge:" + Edge_Index, Trajectory([0, 1], [EdgeA, EdgeB])) vis.hideLabel("PIPEdge:" + Edge_Index, True) vis.setAttribute("PIPEdge:" + Edge_Index, 'width', 7.5) vis.add("PIPEdgeCOM:" + Edge_Index, Trajectory([0, 1], [COM_Pos, EdgeCOM])) vis.hideLabel("PIPEdgeCOM:" + Edge_Index, True) vis.setAttribute("PIPEdgeCOM:" + Edge_Index, 'width', 7.5) vis.setColor("PIPEdgeCOM:" + Edge_Index, 65.0 / 255.0, 199.0 / 255.0, 244.0 / 255.0, 1.0) # Local Coordinates Edgex_i = [0.0, 0.0, 0.0] Edgex_i[0] = EdgeCOM[0] + scale * Edgex[0] Edgex_i[1] = EdgeCOM[1] + scale * Edgex[1] Edgex_i[2] = EdgeCOM[2] + scale * Edgex[2] vis.add("PIPEdgex:" + Edge_Index, Trajectory([0, 1], [EdgeCOM, Edgex_i])) vis.hideLabel("PIPEdgex:" + Edge_Index, True) vis.setAttribute("PIPEdgex:" + Edge_Index, 'width', 7.5) vis.setColor("PIPEdgex:" + Edge_Index, 1.0, 0.0, 0.0, 1.0) Edgey_i = [0.0, 0.0, 0.0] Edgey_i[0] = EdgeCOM[0] + scale * Edgey[0] Edgey_i[1] = EdgeCOM[1] + scale * Edgey[1] Edgey_i[2] = EdgeCOM[2] + scale * Edgey[2] vis.add("PIPEdgey:" + Edge_Index, Trajectory([0, 1], [EdgeCOM, Edgey_i])) vis.hideLabel("PIPEdgey:" + Edge_Index, True) vis.setAttribute("PIPEdgey:" + Edge_Index, 'width', 7.5) vis.setColor("PIPEdgey:" + Edge_Index, 155.0 / 255.0, 244.0 / 255.0, 66.0 / 255.0, 1.0) Edgez_i = [0.0, 0.0, 0.0] Edgez_i[0] = EdgeCOM[0] + scale * Edgez[0] Edgez_i[1] = EdgeCOM[1] + scale * Edgez[1] Edgez_i[2] = EdgeCOM[2] + scale * Edgez[2] # print Edgez_i vis.add("PIPEdgez:" + Edge_Index, Trajectory([0, 1], [EdgeCOM, Edgez_i])) vis.hideLabel("PIPEdgez:" + Edge_Index, True) vis.setAttribute("PIPEdgez:" + Edge_Index, 'width', 7.5) vis.setColor("PIPEdgez:" + Edge_Index, 68.0 / 255.0, 65.0 / 255.0, 244.0 / 255.0, 1.0)
def draw_path(self, pos1, pos2, colour=None): """Draw a path (line) in the klampt visualiser :param pos1: the position of the start of line :param pos2: the position of the end of line :param colour: the colour of the line """ from klampt import vis from klampt.model.trajectory import Trajectory unique_label = repr((pos1, pos2)) label = repr((pos1, pos2)) if label not in self.drawn_label: vis.add(label, Trajectory(milestones=[pos1, pos2]), keepAppearance=True) if colour is not None: vis.setAttribute(label, "color", colour) vis.hideLabel(label)
def visRoadMap(self): vis.show() edgeList = self.G.edges() cnt = 0 for e in edgeList: n1 = e[0] n2 = e[1] tr = trajectory.Trajectory() tr.milestones.append([n1.x, n1.y, n1.z]) tr.milestones.append([n2.x, n2.y, n2.z]) fName = "a" + str(cnt) vis.add(fName, tr) vis.hideLabel(fName) vis.setAttribute(fName, "width", 0.5) vis.setColor(fName, 0.4940, 0.1840, 0.5560) cnt = cnt + 1 self.rmCnt = cnt
def WeightedContactDataPlot(vis, OptimalContact_data, OptimalContactWeights_data): scale = 1.0 for i in range(OptimalContact_data.size / 3): point_start = [0.0, 0.0, 0.0] ReachableContact_i = OptimalContact_data[i] point_start[0] = ReachableContact_i[0] point_start[1] = ReachableContact_i[1] point_start[2] = ReachableContact_i[2] point_end = [0.0, 0.0, 0.0] ReachableContactWeight_i = OptimalContactWeights_data[i] point_end[0] = point_start[0] + scale * ReachableContactWeight_i[0] point_end[1] = point_start[1] + scale * ReachableContactWeight_i[1] point_end[2] = point_start[2] + scale * ReachableContactWeight_i[2] print i vis.add("PointWeights_" + str(i), Trajectory([0, 1], [point_start, point_end])) vis.hideLabel("PointWeights_" + str(i), True) vis.setColor("PointWeights_" + str(i), 0.0, 204.0 / 255.0, 0.0, 1.0) vis.setAttribute("PointWeights_" + str(i), 'width', 5.0)
def so3_grid_test(N=5, staggered=True): from klampt import vis from klampt.model import trajectory if staggered: G = so3_staggered_grid(N) else: G = so3_grid(N) vispt = [1, 0, 0] for n in G.nodes(): R = G.node[n]['params'] trans = so3.apply(R, vispt) #trans = so3.axis_angle(R)[0] #trans = vectorops.unit(so3.quaternion(R)[:3]) vis.add(str(n), [R, trans]) vis.hideLabel(str(n)) #draw edges? minDist = float('inf') maxDist = 0.0 for i, j in G.edges(): Ri = G.node[i]['params'] Rj = G.node[j]['params'] tmax = 9 times = range(tmax + 1) milestones = [] for t in times: u = float(t) / tmax trans = so3.apply(so3.interpolate(Ri, Rj, u), vispt) milestones.append(trans) vis.add( str(i) + '-' + str(j), trajectory.Trajectory(times, milestones)) vis.hideLabel(str(i) + '-' + str(j)) dist = so3.distance(Ri, Rj) if dist > maxDist: maxDist = dist print "Max dispersion at", i, j, ":", dist if dist < minDist: minDist = dist print "Number of points:", G.number_of_nodes() print "Min/max dispersion:", minDist, maxDist vis.run()
def visualize_in_klampt(self): from klampt import vis height = 0.25 for xyz in self.scatter: name = f"{xyz[0]}{xyz[1]}{xyz[2]}" traj = trajectory.Trajectory( milestones=[[xyz[0], xyz[1], xyz[2] + 0.001], [xyz[0], xyz[1], xyz[2] + height]]) vis.add(name, traj) vis.hideLabel(name) for i in range(len(self.scatter) - 4, len(self.scatter)): xyz = self.scatter[i] name = f"{xyz[0]}{xyz[1]}{xyz[2]}_2" traj = trajectory.Trajectory( milestones=[[xyz[0], xyz[1], xyz[2] + 0.001], [xyz[0], xyz[1], xyz[2] + 1]]) vis.add(name, traj) vis.setColor(name, 1, 0, 0, 1)
def ImpulsePlot(vis, SimRobot, SimulationTime, ImpulseObj): StartTime = ImpulseObj[0] EndTime = ImpulseObj[1] ImpulForce = ImpulseObj[2] DurationTime = EndTime - StartTime OriPoint = SimRobot.link(19).getWorldPosition([0.0, 0.0, 0.0]) ImpulseScale = 0.001 * (SimulationTime - StartTime) / DurationTime EndPoint = OriPoint[:] EndPoint[0] += ImpulForce[0] * ImpulseScale EndPoint[1] += ImpulForce[1] * ImpulseScale EndPoint[2] += ImpulForce[2] * ImpulseScale if SimulationTime < StartTime: pass elif SimulationTime > EndTime: vis.hide("Impulse", True) else: vis.add("Impulse", Trajectory([0, 1], [OriPoint, EndPoint])) vis.hideLabel("Impulse", True) vis.setColor("Impulse", 235.0 / 255.0, 52.0 / 255.0, 52.0 / 255.0, 1.0) vis.setAttribute("Impulse", 'width', 5.0)
def WeightedContactDataPlot(vis, StepNo, LimbNo, OptimalContact_data, OptimalContactWeights_data): scale = 1.0 for i in range(OptimalContact_data.size / 3): point_start = [0.0, 0.0, 0.0] ReachableContact_i = OptimalContact_data[i] point_start[0] = ReachableContact_i[0] point_start[1] = ReachableContact_i[1] point_start[2] = ReachableContact_i[2] point_end = [0.0, 0.0, 0.0] ReachableContactWeight_i = OptimalContactWeights_data[i] point_end[0] = point_start[0] + scale * ReachableContactWeight_i[0] point_end[1] = point_start[1] + scale * ReachableContactWeight_i[1] point_end[2] = point_start[2] + scale * ReachableContactWeight_i[2] ContactName = "Stage" + str(StepNo) + "LinkNo" + str( LimbNo) + "Point:" + str(i) print i vis.add(ContactName, Trajectory([0, 1], [point_start, point_end])) vis.hideLabel(ContactName, True) vis.setColor(ContactName, 0.0, 204.0 / 255.0, 0.0, 1.0) vis.setAttribute(ContactName, 'width', 5.0)
def visualize(self, style="none", hide_label=True): if style == "none": milestones = [] for p in list(self.shapely_poly.exterior.coords): z = self.height_map.height_at_xy(p[0], p[1]) + 0.01 milestones.append([p[0], p[1], z]) path = trajectory.Trajectory(milestones=milestones) if not self.name: self.name = "Support Triangle " + str(random.randint(1, 1000)) vis.add(self.name, path) if hide_label: vis.hideLabel(self.name) elif style == "dashed": dashes = 10.0 xyz_points = list(self.shapely_poly.exterior.coords) for i in range(len(xyz_points)): x, y = xyz_points[i][0], xyz_points[i][1] xyz_points[i] = [x, y, self.height_map.height_at_xy(x, y) + 0.01] self.visualize_dashed_line(xyz_points[0], xyz_points[1], "1-2", dashes) self.visualize_dashed_line(xyz_points[0], xyz_points[2], "1-3", dashes) self.visualize_dashed_line(xyz_points[1], xyz_points[2], "2-3", dashes)
print("Saving SDF to",fn) geometryopt.dump_grid_mat(linkgeoms[i].grid,fn) qinit = robot.getConfig() vis.add("qsoln",qinit) vis.setColor("qsoln",0,1,0,0.5) #edit configuration as target vis.edit(("world",robot.getName())) #should we draw? if DRAW_GRID_AND_PC: for i,g in enumerate(constraints[0].robot.geometry): if g is not None: vis.add("grid "+str(i),g.grid) vis.setColor("grid "+str(i),0,1,0,0.5) vis.hideLabel("grid "+str(i)) #vis.add("pc "+str(i),g.pc) #vis.setColor("pc "+str(i),1,1,0,0.5) for i,obs in enumerate(obstaclegeoms): vis.add("envpc "+str(i),obs.pc) vis.setColor("envpc "+str(i),1,1,0,0.5) vis.hideLabel("envpc "+str(i)) settings = None settings = SemiInfiniteOptimizationSettings() #if you use qinit = random-collision-free, you'll want to set this higher settings.max_iters = 5 settings.minimum_constraint_value = -0.02 vis.addPlot("timing")
random.seed(seed) np.random.seed(seed) W.sampleInnerBound(s, i) for i, s in enumerate(outer_sizes): random.seed(seed) np.random.seed(seed) W.sampleOuterBound(s, i + 2) innermax = max(len(W.inner_bounds) for W in hand_wrench_spaces.itervalues()) outermax = max(len(W.outer_bounds) for W in hand_wrench_spaces.itervalues()) decompmax = max( len(W.convex_decomposition) for W in hand_wrench_spaces.itervalues()) assert decompmax == 1, "Can only handle convex wrench spaces at the moment" vis.visualization._frontend.clearColor = (1, 1, 1, 1) vis.add("world", world) vis.hideLabel("world") while True: edit_config() #test_stability_running_time() #break #print test_stability() #print test_stability_traditional() links = hand_types.keys() indices = WrenchSpaceIndices(links, [0] * len(links), 'inner') for i in xrange(innermax): indices.indices = [i] * len(links) sp = compute_support_polygon(indices, (0, 0, -9.8)) if sp is not None: points = [(p[0], p[1], 0) for p in sp] points.append(points[0]) name = "inner " + str(inner_sizes[i])
ctest2.clearx() raw_input("Press enter to continue... ") """ timescale = 10 trajinit.times = [timescale * v for v in trajinit.times] if trajinit is not trajsolved: trajsolved.times = [timescale * v for v in trajsolved.times] vis.add( "Initial trajectory", trajinit.getLinkTrajectory( END_EFFECTOR_LINK, 0.1).getPositionTrajectory(END_EFFECTOR_LOCAL_POSITION)) #vis.add("Initial trajectory",trajinit) vis.setColor("Initial trajectory", 1, 1, 0, 0.5) vis.hideLabel("Initial trajectory") vis.setAttribute("Initial trajectory", "width", 2) eetrajopt = trajsolved.getLinkTrajectory( END_EFFECTOR_LINK, 0.1).getPositionTrajectory(END_EFFECTOR_LOCAL_POSITION) vis.add("Solution trajectory", eetrajopt) #vis.add("Solution trajectory",trajsolved) vis.hideLabel("Solution trajectory") vis.animate(("world", robot.getName()), trajsolved) params[0] = sorted(params[0], key=lambda x: x[2]) for i, p in enumerate(params[0]): link = p[0] env = p[1] t = p[2] wpt = p[3:6] eept = eetrajopt.eval(t * timescale)
def queryPRM(self, qI, qG): vis.show() vis.lock() vis.add("Initial", [qI.x, qI.y, qI.z]) vis.setAttribute("Initial", "size", 14) vis.setColor("Initial", 0, 0.4470, 0.7410) vis.add("Goal", [qG.x, qG.y, qG.z]) vis.setAttribute("Goal", "size", 14) vis.setColor("Goal", 0.8500, 0.3250, 0.0980) vis.unlock() vis.show(False) if (self.localPlanner(qI, qG)): print("Direct path found") cnt = 0 tr = trajectory.Trajectory() tr.milestones.append([qI.x, qI.y, qI.z]) tr.milestones.append([qG.x, qG.y, qG.z]) fName = "p" + str(cnt) vis.add(fName, tr) vis.hideLabel(fName) vis.setColor(fName, 0, 0.4470, 0.7410) cnt = cnt + 1 self.pathCnt = cnt flagIn = True self.env.setConfig(qI.x, qI.y, qI.z, qI.sc, qI.tht) vis.show() while (flagIn): inText = '' while not (inText == 'y' or inText == 'n'): inText = raw_input("Run Path (y/n) ?") if inText == 'n': flagIn = False break if inText == 'y': self.localPlanner(qI, qG, True) return True self.pathCnt = 0 #vis.show(False) nodeList = list(self.G) minNodeI = None minDist = 100000 for nodeG in nodeList: dist = self.distMetric(qI, nodeG) if dist < self.maxDist: if (self.localPlanner(qI, nodeG)): if (minDist > dist): minNodeI = nodeG minDist = dist if minNodeI == None: return False minNodeG = None minDist = 100000 for nodeG in nodeList: dist = self.distMetric(qG, nodeG) if dist < self.maxDist: if (self.localPlanner(qG, nodeG)): if (minDist > dist): minNodeG = nodeG minDist = dist if minNodeG == None: return False try: shortest_path = nx.dijkstra_path(self.G, source=minNodeI, target=minNodeG) except: return False shortest_path = self.smoothPath(shortest_path) vis.show() cnt = 0 tr = trajectory.Trajectory() tr.milestones.append([qI.x, qI.y, qI.z]) tr.milestones.append([minNodeI.x, minNodeI.y, minNodeI.z]) fName = "p" + str(cnt) vis.add(fName, tr) vis.hideLabel(fName) vis.setColor(fName, 0, 0.4470, 0.7410) cnt = cnt + 1 prevNode = None for nodeP in shortest_path: if prevNode == None: prevNode = nodeP continue tr = trajectory.Trajectory() tr.milestones.append([prevNode.x, prevNode.y, prevNode.z]) tr.milestones.append([nodeP.x, nodeP.y, nodeP.z]) prevNode = nodeP fName = "p" + str(cnt) vis.add(fName, tr) vis.hideLabel(fName) vis.setColor(fName, 0.4660, 0.6740, 0.1880) cnt = cnt + 1 tr = trajectory.Trajectory() tr.milestones.append([qG.x, qG.y, qG.z]) tr.milestones.append([minNodeG.x, minNodeG.y, minNodeG.z]) fName = "p" + str(cnt) vis.add(fName, tr) vis.hideLabel(fName) #vis.setAttribute(fName,"width",1.0) vis.setColor(fName, 0.8500, 0.3250, 0.0980) cnt = cnt + 1 self.pathCnt = cnt flagIn = True self.env.setConfig(qI.x, qI.y, qI.z, qI.sc, qI.tht) vis.show() while (flagIn): inText = '' while not (inText == 'y' or inText == 'n'): inText = raw_input("Run Path (y/n) ?") if inText == 'n': flagIn = False break if inText == 'y': self.runPath(qI, minNodeI, minNodeG, qG, shortest_path) return True