Beispiel #1
0
 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)
Beispiel #2
0
    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)
Beispiel #3
0
    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)
Beispiel #4
0
 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)
Beispiel #5
0
 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')
Beispiel #6
0
 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)
Beispiel #7
0
    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)
Beispiel #8
0
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]
Beispiel #9
0
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