def visualize15z(self): pointlist = [] for i in self.nodelist: ( q1, q2, q3, q4, q5 ) = i.config[0], i.config[1], i.config[2], i.config[3], i.config[4] z = Pz(q1, q2, q3, q4, q5) pointlist.append([q1, q5, z]) np_pointlist = np.array(pointlist) q1list = np_pointlist[:, 0] q5list = np_pointlist[:, 1] zlist = np_pointlist[:, 2] fig = plt.figure() ax = fig.gca(projection='3d') ax.scatter(q1list, q5list, zlist) for i in self.connection_idx: config_a = self.nodelist[i[0]].config config_b = self.nodelist[i[1]].config z_1 = Pz(config_a[0], config_a[1], config_a[2], config_a[3], config_a[4]) z_2 = Pz(config_b[0], config_b[1], config_b[2], config_b[3], config_b[4]) q1 = [config_a[0], config_b[0]] q5 = [config_a[4], config_b[4]] z = [z_1, z_2] ax.plot(q1, q5, z, color='red') plt.show(ax)
def visualizexyz(self): xlist = [] ylist = [] zlist = [] for i in self.nodelist: (q1, q2, q3, q4, q5) = i.config[0], i.config[1], i.config[2], i.config[3], i.config[4] x = Px(q1, q2, q3, q4, q5) y = Py(q1, q2, q3, q4, q5) z = Pz(q1, q2, q3, q4, q5) xlist.append(x) ylist.append(y) zlist.append(z) x_np = np.array(xlist) y_np = np.array(ylist) z_np = np.array(zlist) fig = plt.figure() ax = fig.gca(projection='3d') ax.scatter(x_np, y_np, z_np) for i in self.obstaclelist: xmin,ymin,zmin = i.twopoint[0] xmax,ymax,zmax = i.twopoint[1] sq1 = np.array([[xmin,ymin,zmin],[xmin,ymin,zmax],[xmax,ymin,zmax],[xmax,ymin,zmin]]) sq2 = np.array([[xmin,ymin,zmin],[xmin,ymax,zmin],[xmax,ymax,zmin],[xmax,ymin,zmin]]) sq3 = np.array([[xmin,ymin,zmin],[xmin,ymin,zmax],[xmin,ymax,zmax],[xmin,ymax,zmin]]) sq4 = np.array([[xmin,ymin,zmax],[xmin,ymax,zmax],[xmax,ymax,zmax],[xmax,ymin,zmax]]) sq5 = np.array([[xmax,ymin,zmin],[xmax,ymax,zmin],[xmax,ymax,zmax],[xmax,ymin,zmax]]) sq6 = np.array([[xmin,ymax,zmin],[xmax,ymax,zmin],[xmax,ymax,zmax],[xmin,ymax,zmax]]) squarelist = [sq1,sq2,sq3,sq4,sq5,sq6] for i in squarelist: x = i[:,0] y = i[:,1] z = i[:,2] verts = [list(zip(x,y,z))] pc = Poly3DCollection(verts,facecolors='g') line = Line3DCollection(verts, colors='k', linewidths=0.5) # ax.add_collection3d(pc) ax.add_collection(line) for i in self.connection_idx: config_a = self.nodelist[i[0]].config config_b = self.nodelist[i[1]].config x_1 = Px(config_a[0],config_a[1],config_a[2],config_a[3],config_a[4]) y_1 = Py(config_a[0],config_a[1],config_a[2],config_a[3],config_a[4]) z_1 = Pz(config_a[0],config_a[1],config_a[2],config_a[3],config_a[4]) x_2 = Px(config_b[0],config_b[1],config_b[2],config_b[3],config_b[4]) y_2 = Py(config_b[0],config_b[1],config_b[2],config_b[3],config_b[4]) z_2 = Pz(config_b[0],config_b[1],config_b[2],config_b[3],config_b[4]) x = [x_1,x_2] y = [y_1,y_2] z = [z_1,z_2] ax.plot(x,y,z,color='yellow') plt.show(ax)
def put_node(self, Node): (q1, q2, q3, q4, q5) = Node.config[0], Node.config[1], Node.config[ 2], Node.config[3], Node.config[4] x = Px(q1, q2, q3, q4, q5) y = Py(q1, q2, q3, q4, q5) z = Pz(q1, q2, q3, q4, q5) intersect = 0 if z < 30 or x > 740: intersect = 1 if x > 30: if y > 510 or y < -510: intersect = 1 if intersect == 0: for i in self.obstaclelist: if i.twopoint[0][0] < x and i.twopoint[1][0] > x and i.twopoint[ 0][1] < y and i.twopoint[1][1] > y and i.twopoint[0][ 2] < z and i.twopoint[1][2] > z: intersect = 1 # print('is in obstacle') break if (q1, q2, q3, q4, q5) == (0, 0, 0, 0, 0): intersect = 0 # print(intersect) if intersect == 0: self.nodelist.append(Node)
def visualize15zpath(self, q_init, q_goal): path = self.astar(q_init, q_goal) print(path) pointlist = [] for i in self.nodelist: ( q1, q2, q3, q4, q5 ) = i.config[0], i.config[1], i.config[2], i.config[3], i.config[4] z = Pz(q1, q2, q3, q4, q5) pointlist.append([q1, q5, z]) np_pointlist = np.array(pointlist) q1list = np_pointlist[:, 0] q5list = np_pointlist[:, 1] zlist = np_pointlist[:, 2] fig = plt.figure() ax = fig.gca(projection='3d') ax.scatter(q1list, q5list, zlist) for i in self.connection_idx: config_a = self.nodelist[i[0]].config config_b = self.nodelist[i[1]].config z_1 = Pz(config_a[0], config_a[1], config_a[2], config_a[3], config_a[4]) z_2 = Pz(config_b[0], config_b[1], config_b[2], config_b[3], config_b[4]) q1 = [config_a[0], config_b[0]] q5 = [config_a[4], config_b[4]] z = [z_1, z_2] ax.plot(q1, q5, z, color='yellow') for i in range(len(path) - 1): q1_1 = path[i][0] q5_1 = path[i][4] z_1 = Pz(path[i][0], path[i][1], path[i][2], path[i][3], path[i][4]) q1_2 = path[i + 1][0] q5_2 = path[i + 1][4] z_2 = Pz(path[i + 1][0], path[i + 1][1], path[i + 1][2], path[i + 1][3], path[i + 1][4]) q1 = [q1_1, q1_2] q5 = [q5_1, q5_2] z = [z_1, z_2] ax.plot(q1, q5, z, color='red') plt.show(ax)
def connect_graph(self): # i = 0 for a in range(len(self.nodelist)): for b in range(len(self.nodelist)): if self.nodelist[a] == self.nodelist[b]: continue if (b, a) in self.connection_idx or (a, b) in self.connection_idx: # print('in index') continue no_colission = True q1 = np.array(self.nodelist[a].config) q2 = np.array(self.nodelist[b].config) diff = q1 - q2 dis_s = np.square(diff) sum = np.sum(dis_s) dis = np.sqrt(sum) if dis > 1.8: # print('check1') continue percentile = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9] for c in percentile: config = q1 + c * (q2 - q1) q1_i, q2_i, q3_i, q4_i, q5_i = config.tolist() # print(q1_i,q2_i,q3_i,q4_i,q5_i) x = Px(q1_i, q2_i, q3_i, q4_i, q5_i) y = Py(q1_i, q2_i, q3_i, q4_i, q5_i) z = Pz(q1_i, q2_i, q3_i, q4_i, q5_i) # print(x,y,z) for d in self.obstaclelist: # print(d.twopoint) if d.twopoint[0][0] <= x and d.twopoint[1][ 0] >= x and d.twopoint[0][ 1] <= y and d.twopoint[1][ 1] >= y and d.twopoint[0][ 2] <= z and d.twopoint[1][2] >= z: no_colission = False break if no_colission == False: break if self.nodelist[a].config == [ 0, 0, 0, 0, 0 ] or self.nodelist[b].config == [0, 0, 0, 0, 0]: no_colission = True # print(no_colission) if no_colission: # print('check1') # print('connect') self.connection_idx.append((a, b)) self.nodelist[a].connectedNode.append(b) self.nodelist[b].connectedNode.append(a) # i+=1 # print(i) print('done')
def put_node(self, Node): (q1, q2, q3, q4, q5) = Node.config[0], Node.config[1], Node.config[ 2], Node.config[3], Node.config[4] x = Px(q1, q2, q3, q4, q5) y = Py(q1, q2, q3, q4, q5) z = Pz(q1, q2, q3, q4, q5) intersect = 0 if z < 0: intersect = 1 if intersect == 0: for i in self.obstaclelist: if i.twopoint[0][0] < x and i.twopoint[1][0] > x and i.twopoint[ 0][1] < y and i.twopoint[1][1] > y and i.twopoint[0][ 2] < z and i.twopoint[1][2] > z: intersect = 1 break if intersect == 0: self.nodelist.append(Node)
def connect_graph(self): for a in range(len(self.nodelist)): for b in range(len(self.nodelist)): if self.nodelist[a] == self.nodelist[b] or ( b, a) in self.connection_idx or ( a, b) in self.connection_idx: continue no_colission = True q1 = np.array(self.nodelist[a].config) q2 = np.array(self.nodelist[b].config) diff = q1 - q2 dis_s = np.square(diff) sum = np.sum(dis_s) dis = np.sqrt(sum) if dis > 7: continue q0_1, q0_2, q0_3, q0_4, q0_5 = self.nodelist[a].config q1_1, q1_2, q1_3, q1_4, q1_5 = self.nodelist[b].config x0 = Px(q0_1, q0_2, q0_3, q0_4, q0_5) y0 = Py(q0_1, q0_2, q0_3, q0_4, q0_5) z0 = Pz(q0_1, q0_2, q0_3, q0_4, q0_5) x1 = Px(q1_1, q1_2, q1_3, q1_4, q1_5) y1 = Py(q1_1, q1_2, q1_3, q1_4, q1_5) z1 = Pz(q1_1, q1_2, q1_3, q1_4, q1_5) P0 = np.array([x0, y0, z0]) P1 = np.array([x1, y1, z1]) #check if line is parallel to any axis pdif = P1 - P0 non_zero = np.count_nonzero(pdif) if non_zero != 3: continue for c in self.obstaclelist: #find whether P0 or P1 is closer to the obstacle dis0 = [] dis1 = [] for d in c.vertices: dis_e = sqrt((P0[0] - d[0])**2 + (P0[1] - d[1])**2 + (P0[2] - d[2])**2) dis_e1 = sqrt((P1[0] - d[0])**2 + (P1[1] - d[1])**2 + (P1[2] - d[2])**2) dis0.append(dis_e) dis1.append(dis_e1) mindis0 = min(dis0) mindis1 = min(dis1) if mindis1 < mindis0: temp = P0 P0 = P1 P1 = temp dis0 = dis1 #find the nearest and farthest vertices idx_f0 = dis0.index(min(dis0)) idx_f1 = dis0.index(max(dis0)) f0 = c.vertices[idx_f0] f1 = c.vertices[idx_f1] print(f0, f1) #find time representation of collision at each plane t0_x = ((f0[0] - P0[0]) / (P1[0] - P0[0])) t0_y = ((f0[1] - P0[1]) / (P1[1] - P0[1])) t0_z = ((f0[2] - P0[2]) / (P1[2] - P0[2])) t1_x = ((f1[0] - P0[0]) / (P1[0] - P0[0])) t1_y = ((f1[1] - P0[1]) / (P1[1] - P0[1])) t1_z = ((f1[2] - P0[2]) / (P1[2] - P0[2])) t0list = [t0_x, t0_y, t0_z] t1list = [t1_x, t1_y, t1_z] t0 = max(t0list) t1 = min(t1list) if t0 <= t1: no_colission = False break # print(no_colission) if no_colission == True: self.connection_idx.append((a, b)) self.nodelist[a].connectedNode.append(b) self.nodelist[b].connectedNode.append(a)
from math import pi from random import uniform from InverseKinematics import inverseKinematics from Node import Node from ForwardKinematics import Px,Py,Pz import pickle with open('Graph.gph','rb') as Graph1_file: Graph1 = pickle.load(Graph1_file) x = Px(0,0,0,0,0) y = Py(0,0,0,0,0) z = Pz(0,0,0,0,0) print(x,y,z) q1,q2,q3,q4,q5 = inverseKinematics(-380, -632, 510, -pi / 2) # print(q1,q2,q3,q4,q5) q1_f,q2_f,q3_f,q4_f,q5_f = inverseKinematics(436.76, -303.78, 706.66, 0) Graph1.visualizexyz_path([-1.0308961180703955, 0.0925216937435418, 1.0356925838733066, -1.1282142776168484, 0.27450632311632295],[q1_f,q2_f,q3_f,q4_f,q5_f]) # Graph1.visualizexyz_path([q1,q2,q3,q4,q5],[q1_f,q2_f,q3_f,q4_f,q5_f]) # print(path) # a = [] # # Graph1.put_node(Node()) # q1, q2, q3, q4, q5 = inverseKinematics(611.74, 411.22, 776.66, 0) # # q1, q2, q3, q4, q5 = inverseKinematics(611.74, 500, 100, 0) # Graph1.visualizexyz_path([1.998590624097611, -1.1340952935402997, 0.4191005098991618, 0.7149947836411379, 1.1429020294925154],[q1,q2,q3,q4,q5]) # path = Graph1.astar([1.998590624097611, -1.1340952935402997, 0.41910050/98991618, 0.7149947836411379, 1.1429020294925154],[0.12283523887789499, -1.6068121493671421, 0.11566883679923867, 1.4911433125679034, -0.12283523887789499]) # path = Graph1.astar([2.0076841941291534, -1.1829360522808943, 0.2995397541333982, 0.8833962981474961, 0.5882534049336479] ,[q1,q2,q3,q4,q5]) # Graph1.visualizexyz() # print(path) # 10 initial_box : [1.998590624097611, -1.1340952935402997, 0.4191005098991618, 0.7149947836411379, 1.1429020294925154] box_pos : [0.12283523887789499, -1.6068121493671421, 0.11566883679923867, 1.4911433125679034, -0.12283523887789499]
Graph1.put_obstacle(obstacle9) Graph1.put_obstacle(obstacle10) Graph1.put_obstacle(obstacle11) Graph1.put_obstacle(obstacle12) Graph1.put_obstacle(obstacle13) # Graph1.put_obstacle(base1) while len(Graph1.nodelist) < 230: q1 = uniform(-140 * pi / 180, 140 * pi / 180) q2 = uniform(-185 / 180 * pi, 20 / 180 * pi) q3 = uniform(-19 / 180 * pi, 3 / 2 * pi) q4 = -q2 - q3 q5 = uniform(-80 * pi / 180, 80 * pi / 180) # print(q1,q2,q3,q4,q5) x5 = Px(q1, q2, q3, q4, q5) - 83 * cos(q1 + q5) y5 = Py(q1, q2, q3, q4, q5) - 83 * sin(q1 + q5) z = Pz(q1, q2, q3, q4, q5) r = sqrt(x5**2 + y5**2) if q2 + 2 * pi / 3 < atan2(z, r): continue if q4 > (-2 / 3 * pi) and q4 < (2 / 3 * pi): Node_i = Node([q1, q2, q3, q4, q5]) Graph1.put_node(Node_i) print(len(Graph1.nodelist)) while len(Graph1.nodelist) < 360: x = uniform(200, 550) y = uniform(-500, 500) z = uniform(40, 800) q1, q2, q3, q4, q5 = inverseKinematics(x, y, z, 0) if q2 > pi / 9 or q2 < -185 / 180 * pi or q3 > pi / 9 or q3 < -19 * pi / 180 or abs( q4) > 120 * pi / 180 or abs(q5) > 75: continue