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
