#ax.grid(True, which='both', axis='both')
ax.axhline(y=0, color='k')
ax.axvline(x=0, color='k')
# Axes labels and limiters
ax.set_xlabel('Ex')
ax.set_ylabel('Ey')
ax.set_ylim(min(Ey), max(Ey))
ax.set_xlim(min(Ex), max(Ex))
ax.set_aspect('equal', adjustable='box')
# Plot red-dotted outline
plt.plot(Ex, Ey, 'r:')
# Animate arrows
patch = patches.Arrow(0, 0, Ex[0], Ey[0], width=0.5)
def init():
ax.add_patch(patch)
return patch,
def animate(t):
ax.patches.pop()
patch = plt.Arrow(0, 0, Ex[t], Ey[t], width=0.5)
ax.add_patch(patch)
return patch,
def HybridAStarPlan(visualize_flag):
# initialze object
HybridAStar = HybridAStarPlanner()
# parameter(except max, min and car size is defined in proto)
num_output_buffer = 100000
sx = -8
sy = 4
sphi = 0.0
scenario = "backward"
# scenario = "parallel"
if scenario == "backward":
# for parking space 11543 in sunnyvale_with_two_offices
left_boundary_x = (c_double *
3)(*[-13.6407054776, 0.0, 0.0515703622475])
left_boundary_y = (c_double *
3)(*[0.0140634663703, 0.0, -5.15258191624])
down_boundary_x = (c_double * 2)(*[0.0515703622475, 2.8237895441])
down_boundary_y = (c_double * 2)(*[-5.15258191624, -5.15306980547])
right_boundary_x = (c_double *
3)(*[2.8237895441, 2.7184833539, 16.3592013995])
right_boundary_y = (c_double * 3)(
*[-5.15306980547, -0.0398078878812, -0.011889513383])
up_boundary_x = (c_double * 2)(*[16.3591910364, -13.6406951857])
up_boundary_y = (c_double * 2)(*[5.60414234644, 5.61797800844])
# obstacles(x, y, size)
HybridAStar.AddVirtualObstacle(left_boundary_x, left_boundary_y, 3)
HybridAStar.AddVirtualObstacle(down_boundary_x, down_boundary_y, 2)
HybridAStar.AddVirtualObstacle(right_boundary_x, right_boundary_y, 3)
HybridAStar.AddVirtualObstacle(up_boundary_x, up_boundary_y, 2)
ex = 1.359
ey = -3.86443643718
ephi = 1.581
XYbounds = [
-13.6406951857, 16.3591910364, -5.15258191624, 5.61797800844
]
x = (c_double * num_output_buffer)()
y = (c_double * num_output_buffer)()
phi = (c_double * num_output_buffer)()
v = (c_double * num_output_buffer)()
a = (c_double * num_output_buffer)()
steer = (c_double * num_output_buffer)()
size = (c_ushort * 1)()
XYbounds_ctype = (c_double * 4)(*XYbounds)
start = time.time()
print("planning start")
success = True
if not HybridAStar.Plan(sx, sy, sphi, ex, ey, ephi, XYbounds_ctype):
print("planning fail")
success = False
end = time.time()
planning_time = end - start
print("planning time is " + str(planning_time))
# load result
x_out = []
y_out = []
phi_out = []
v_out = []
a_out = []
steer_out = []
if visualize_flag and success:
HybridAStar.GetResult(x, y, phi, v, a, steer, size)
for i in range(0, size[0]):
x_out.append(float(x[i]))
y_out.append(float(y[i]))
phi_out.append(float(phi[i]))
v_out.append(float(v[i]))
a_out.append(float(a[i]))
steer_out.append(float(steer[i]))
# plot
fig1 = plt.figure(1)
ax = fig1.add_subplot(111)
for i in range(0, size[0]):
downx = 1.055 * math.cos(phi_out[i] - math.pi / 2)
downy = 1.055 * math.sin(phi_out[i] - math.pi / 2)
leftx = 1.043 * math.cos(phi_out[i] - math.pi)
lefty = 1.043 * math.sin(phi_out[i] - math.pi)
x_shift_leftbottom = x_out[i] + downx + leftx
y_shift_leftbottom = y_out[i] + downy + lefty
car = patches.Rectangle((x_shift_leftbottom, y_shift_leftbottom),
3.89 + 1.043,
1.055 * 2,
angle=phi_out[i] * 180 / math.pi,
linewidth=1,
edgecolor='r',
facecolor='none')
arrow = patches.Arrow(x_out[i], y_out[i],
0.25 * math.cos(phi_out[i]),
0.25 * math.sin(phi_out[i]), 0.2)
ax.add_patch(car)
ax.add_patch(arrow)
ax.plot(sx, sy, "s")
ax.plot(ex, ey, "s")
if scenario == "backward":
left_boundary_x = [-13.6407054776, 0.0, 0.0515703622475]
left_boundary_y = [0.0140634663703, 0.0, -5.15258191624]
down_boundary_x = [0.0515703622475, 2.8237895441]
down_boundary_y = [-5.15258191624, -5.15306980547]
right_boundary_x = [2.8237895441, 2.7184833539, 16.3592013995]
right_boundary_y = [
-5.15306980547, -0.0398078878812, -0.011889513383
]
up_boundary_x = [16.3591910364, -13.6406951857]
up_boundary_y = [5.60414234644, 5.61797800844]
ax.plot(left_boundary_x, left_boundary_y, "k")
ax.plot(down_boundary_x, down_boundary_y, "k")
ax.plot(right_boundary_x, right_boundary_y, "k")
ax.plot(up_boundary_x, up_boundary_y, "k")
plt.axis('equal')
fig2 = plt.figure(2)
v_graph = fig2.add_subplot(311)
v_graph.title.set_text('v')
v_graph.plot(np.linspace(0, size[0], size[0]), v_out)
a_graph = fig2.add_subplot(312)
a_graph.title.set_text('a')
a_graph.plot(np.linspace(0, size[0], size[0]), a_out)
steer_graph = fig2.add_subplot(313)
steer_graph.title.set_text('steering')
steer_graph.plot(np.linspace(0, size[0], size[0]), steer_out)
plt.show()
if not visualize_flag:
if success:
HybridAStar.GetResult(x, y, phi, v, a, steer, size)
for i in range(0, size[0]):
x_out.append(float(x[i]))
y_out.append(float(y[i]))
phi_out.append(float(phi[i]))
v_out.append(float(v[i]))
a_out.append(float(a[i]))
steer_out.append(float(steer[i]))
return success, x_out, y_out, phi_out, v_out, a_out, steer_out, planning_time
def SmoothTrajectory(visualize_flag, sx, sy):
# initialze object
OpenSpacePlanner = DistancePlanner()
# parameter(except max, min and car size is defined in proto)
num_output_buffer = 10000
# sx = -8
# sy = 1.5
# sphi = 0.5
sphi = 0.0
scenario = "backward"
# scenario = "parallel"
if scenario == "backward":
# obstacles for distance approach(vertices coords in clock wise order)
ROI_distance_approach_parking_boundary = (c_double * 20)(*[
-13.6407054776,
0.0140634663703,
0.0,
0.0,
0.0515703622475,
-5.15258191624,
0.0515703622475,
-5.15258191624,
2.8237895441,
-5.15306980547,
2.8237895441,
-5.15306980547,
2.7184833539,
-0.0398078878812,
16.3592013995,
-0.011889513383,
16.3591910364,
5.60414234644,
-13.6406951857,
5.61797800844,
])
OpenSpacePlanner.AddObstacle(ROI_distance_approach_parking_boundary)
# parking lot position
ex = 1.359
ey = -3.86443643718
ephi = 1.581
XYbounds = [
-13.6406951857, 16.3591910364, -5.15258191624, 5.61797800844
]
x = (c_double * num_output_buffer)()
y = (c_double * num_output_buffer)()
phi = (c_double * num_output_buffer)()
v = (c_double * num_output_buffer)()
a = (c_double * num_output_buffer)()
steer = (c_double * num_output_buffer)()
opt_x = (c_double * num_output_buffer)()
opt_y = (c_double * num_output_buffer)()
opt_phi = (c_double * num_output_buffer)()
opt_v = (c_double * num_output_buffer)()
opt_a = (c_double * num_output_buffer)()
opt_steer = (c_double * num_output_buffer)()
opt_time = (c_double * num_output_buffer)()
opt_dual_l = (c_double * num_output_buffer)()
opt_dual_n = (c_double * num_output_buffer)()
size = (c_ushort * 1)()
XYbounds_ctype = (c_double * 4)(*XYbounds)
hybrid_time = (c_double * 1)(0.0)
dual_time = (c_double * 1)(0.0)
ipopt_time = (c_double * 1)(0.0)
success = True
start = time.time()
print("planning start")
if not OpenSpacePlanner.DistancePlan(sx, sy, sphi, ex, ey, ephi,
XYbounds_ctype):
print("planning fail")
success = False
# exit()
planning_time = time.time() - start
print("planning time is " + str(planning_time))
x_out = []
y_out = []
phi_out = []
v_out = []
a_out = []
steer_out = []
opt_x_out = []
opt_y_out = []
opt_phi_out = []
opt_v_out = []
opt_a_out = []
opt_steer_out = []
opt_time_out = []
opt_dual_l_out = []
opt_dual_n_out = []
if visualize_flag and success:
# load result
OpenSpacePlanner.DistanceGetResult(x, y, phi, v, a, steer, opt_x,
opt_y, opt_phi, opt_v, opt_a,
opt_steer, opt_time, opt_dual_l,
opt_dual_n, size, hybrid_time,
dual_time, ipopt_time)
for i in range(0, size[0]):
x_out.append(float(x[i]))
y_out.append(float(y[i]))
phi_out.append(float(phi[i]))
v_out.append(float(v[i]))
a_out.append(float(a[i]))
steer_out.append(float(steer[i]))
opt_x_out.append(float(opt_x[i]))
opt_y_out.append(float(opt_y[i]))
opt_phi_out.append(float(opt_phi[i]))
opt_v_out.append(float(opt_v[i]))
opt_a_out.append(float(opt_a[i]))
opt_steer_out.append(float(opt_steer[i]))
opt_time_out.append(float(opt_time[i]))
for i in range(0, size[0] * 6):
opt_dual_l_out.append(float(opt_dual_l[i]))
for i in range(0, size[0] * 16):
opt_dual_n_out.append(float(opt_dual_n[i]))
# trajectories plot
fig1 = plt.figure(1)
ax = fig1.add_subplot(111)
for i in range(0, size[0]):
# warm start
downx = 1.055 * math.cos(phi_out[i] - math.pi / 2)
downy = 1.055 * math.sin(phi_out[i] - math.pi / 2)
leftx = 1.043 * math.cos(phi_out[i] - math.pi)
lefty = 1.043 * math.sin(phi_out[i] - math.pi)
x_shift_leftbottom = x_out[i] + downx + leftx
y_shift_leftbottom = y_out[i] + downy + lefty
warm_start_car = patches.Rectangle(
(x_shift_leftbottom, y_shift_leftbottom),
3.89 + 1.043,
1.055 * 2,
angle=phi_out[i] * 180 / math.pi,
linewidth=1,
edgecolor='r',
facecolor='none')
warm_start_arrow = patches.Arrow(
x_out[i],
y_out[i],
0.25 * math.cos(phi_out[i]),
0.25 * math.sin(phi_out[i]),
0.2,
edgecolor='r',
)
# ax.add_patch(warm_start_car)
ax.add_patch(warm_start_arrow)
# distance approach
downx = 1.055 * math.cos(opt_phi_out[i] - math.pi / 2)
downy = 1.055 * math.sin(opt_phi_out[i] - math.pi / 2)
leftx = 1.043 * math.cos(opt_phi_out[i] - math.pi)
lefty = 1.043 * math.sin(opt_phi_out[i] - math.pi)
x_shift_leftbottom = opt_x_out[i] + downx + leftx
y_shift_leftbottom = opt_y_out[i] + downy + lefty
smoothing_car = patches.Rectangle(
(x_shift_leftbottom, y_shift_leftbottom),
3.89 + 1.043,
1.055 * 2,
angle=opt_phi_out[i] * 180 / math.pi,
linewidth=1,
edgecolor='y',
facecolor='none')
smoothing_arrow = patches.Arrow(
opt_x_out[i],
opt_y_out[i],
0.25 * math.cos(opt_phi_out[i]),
0.25 * math.sin(opt_phi_out[i]),
0.2,
edgecolor='y',
)
ax.add_patch(smoothing_car)
ax.add_patch(smoothing_arrow)
ax.plot(sx, sy, "s")
ax.plot(ex, ey, "s")
if scenario == "backward":
left_boundary_x = [-13.6407054776, 0.0, 0.0515703622475]
left_boundary_y = [0.0140634663703, 0.0, -5.15258191624]
down_boundary_x = [0.0515703622475, 2.8237895441]
down_boundary_y = [-5.15258191624, -5.15306980547]
right_boundary_x = [2.8237895441, 2.7184833539, 16.3592013995]
right_boundary_y = [
-5.15306980547, -0.0398078878812, -0.011889513383
]
up_boundary_x = [16.3591910364, -13.6406951857]
up_boundary_y = [5.60414234644, 5.61797800844]
ax.plot(left_boundary_x, left_boundary_y, "k")
ax.plot(down_boundary_x, down_boundary_y, "k")
ax.plot(right_boundary_x, right_boundary_y, "k")
ax.plot(up_boundary_x, up_boundary_y, "k")
plt.axis('equal')
# input plot
fig2 = plt.figure(2)
v_graph = fig2.add_subplot(411)
v_graph.title.set_text('v')
v_graph.plot(np.linspace(0, size[0], size[0]), v_out)
v_graph.plot(np.linspace(0, size[0], size[0]), opt_v_out)
a_graph = fig2.add_subplot(412)
a_graph.title.set_text('a')
a_graph.plot(np.linspace(0, size[0], size[0]), a_out)
a_graph.plot(np.linspace(0, size[0], size[0]), opt_a_out)
steer_graph = fig2.add_subplot(413)
steer_graph.title.set_text('steering')
steer_graph.plot(np.linspace(0, size[0], size[0]), steer_out)
steer_graph.plot(np.linspace(0, size[0], size[0]), opt_steer_out)
steer_graph = fig2.add_subplot(414)
steer_graph.title.set_text('t')
steer_graph.plot(np.linspace(0, size[0], size[0]), opt_time_out)
# dual variables
fig3 = plt.figure(3)
dual_l_graph = fig3.add_subplot(211)
dual_l_graph.title.set_text('dual_l')
dual_l_graph.plot(np.linspace(0, size[0] * 6, size[0] * 6),
opt_dual_l_out)
dual_n_graph = fig3.add_subplot(212)
dual_n_graph.title.set_text('dual_n')
dual_n_graph.plot(np.linspace(0, size[0] * 16, size[0] * 16),
opt_dual_n_out)
plt.show()
return True
if not visualize_flag:
if success:
# load result
OpenSpacePlanner.DistanceGetResult(x, y, phi, v, a, steer, opt_x,
opt_y, opt_phi, opt_v, opt_a,
opt_steer, opt_time, opt_dual_l,
opt_dual_n, size, hybrid_time,
dual_time, ipopt_time)
for i in range(0, size[0]):
x_out.append(float(x[i]))
y_out.append(float(y[i]))
phi_out.append(float(phi[i]))
v_out.append(float(v[i]))
a_out.append(float(a[i]))
steer_out.append(float(steer[i]))
opt_x_out.append(float(opt_x[i]))
opt_y_out.append(float(opt_y[i]))
opt_phi_out.append(float(opt_phi[i]))
opt_v_out.append(float(opt_v[i]))
opt_a_out.append(float(opt_a[i]))
opt_steer_out.append(float(opt_steer[i]))
opt_time_out.append(float(opt_time[i]))
# check end_pose distacne
end_pose_dist = math.sqrt((opt_x_out[-1] - ex)**2 +
(opt_y_out[-1] - ey)**2)
end_pose_heading = abs(opt_phi_out[-1] - ephi)
reach_end_pose = (end_pose_dist <= 0.1
and end_pose_heading <= 0.17)
else:
end_pose_dist = 100.0
end_pose_heading = 100.0
reach_end_pose = 0
return [
success, end_pose_dist, end_pose_heading, reach_end_pose,
opt_x_out, opt_y_out, opt_phi_out, opt_v_out, opt_a_out,
opt_steer_out, opt_time_out, hybrid_time, dual_time, ipopt_time,
planning_time
]
return False
label(grid[2], "Wedge")
# add a Polygon
polygon = mpatches.RegularPolygon(grid[3], 5, 0.1)
patches.append(polygon)
label(grid[3], "Polygon")
#add an ellipse
ellipse = mpatches.Ellipse(grid[4], 0.2, 0.1)
patches.append(ellipse)
label(grid[4], "Ellipse")
#add an arrow
arrow = mpatches.Arrow(grid[5, 0] - 0.05,
grid[5, 1] - 0.05,
0.1,
0.1,
width=0.1)
patches.append(arrow)
label(grid[5], "Arrow")
# add a path patch
Path = mpath.Path
path_data = [(Path.MOVETO, [0.018, -0.11]), (Path.CURVE4, [-0.031, -0.051]),
(Path.CURVE4, [-0.115, 0.073]), (Path.CURVE4, [-0.03, 0.073]),
(Path.LINETO, [-0.011, 0.039]), (Path.CURVE4, [0.043, 0.121]),
(Path.CURVE4, [0.075, -0.005]), (Path.CURVE4, [0.035, -0.027]),
(Path.CLOSEPOLY, [0.018, -0.11])]
codes, verts = zip(*path_data)
path = mpath.Path(verts + grid[6], codes)
patch = mpatches.PathPatch(path)
def plotStresses(self,plotCir,plotVel,plotCon,plotF,plotStress,**kwargs):
if 'figure' in kwargs:
fig=plt.figure(figsize=(20,20))
#fig=plt.figure()
axval= fig.add_subplot(1, 1, 1)
elif 'axis' in kwargs:
axval=kwargs['axis']
else:
fig=plt.figure()
axval= fig.add_subplot(1, 1, 1)
if 'timestamp' in kwargs:
axval.text(0.0,0.4*self.Ly,'T= ' +str(kwargs['timestamp']),fontsize=18)
for k in range(self.N):
# Particle circles
if (plotCir):
cir1=ptch.Circle((self.conf.x[k],self.conf.y[k]),radius=self.conf.rad[k],ec=(0.5, 0.5, 0.5),fc='none', linewidth=2)
axval.add_patch(cir1)
#angline=lne.Line2D([self.x[k],self.x[k]+self.rad[k]*np.cos(self.alpha[k])],[self.y[k],self.y[k]+self.rad[k]*np.sin(self.alpha[k])],color=(0.5, 0.5, 0.5))
#plt.gca().add_line(angline)
if self.verbose:
axval.text(self.conf.x[k],self.conf.y[k],str(k),fontsize=8)
# Velocity vectors
if (plotVel):
scale=2*np.mean(self.conf.rad)/np.sqrt(np.mean(self.conf.dx*self.conf.dx)+np.mean(self.conf.dy*self.conf.dy))
velvect=ptch.Arrow(self.conf.x[k],self.conf.y[k],scale*self.conf.dx[k],scale*self.conf.dy[k],width=0.3,color='b')
axval.add_patch(velvect)
# Plotting contacts
if (plotCon):
for k in range(len(self.conf.I)):
x0,x1,y0,y1=self.conf.getConPos(k)
if (self.fullmobi[k]==1):
conlink=lne.Line2D([x0,x1],[y0,y1],color=(0.7,0,0),lw=3.0)
axval.add_line(conlink)
else:
conlink=lne.Line2D([x0,x1],[y0,y1],color=(0,0,0),lw=3.0)
axval.add_line(conlink)
if self.verbose:
if (k>=self.ncon):
axval.text(x0+0.5*(x1-x0)-self.small*np.sin(ang),y0+0.5*(y1-y0)+self.small*np.cos(ang),str(k),fontsize=8)
else:
axval.text(x0+0.5*(x1-x0),y0+0.5*(y1-y0),str(k),fontsize=8)
if (plotF):
#fscale=np.mean(self.fnor)
fscale=self.fgiven
Fcolor,Fmap=self.color_init(np.sqrt(np.amax(self.conf.fnor))/fscale)
for k in range(len(self.conf.I)):
fval=np.sqrt(self.conf.fnor[k]/fscale)
x0,x1,y0,y1=self.conf.getConPos(k)
conlink=lne.Line2D([x0,x1],[y0,y1],color=Fcolor(fval),lw=2*fval)
axval.add_line(conlink)
# if self.verbose:
# if (k>=self.ncon):
# axval.text(x0+0.5*(x1-x0)-self.small*np.sin(ang),y0+0.5*(y1-y0)+self.small*np.cos(ang),str(k),fontsize=8)
# else:
# axval.text(x0+0.5*(x1-x0),y0+0.5*(y1-y0),str(k),fontsize=8)
if (plotStress):
print("Plot Stress is still under construction.")
plt.title('Forces and contacts')
axval.axis('scaled')
if self.conf.datatype=='simulation':
axval.set_xlim(-self.Lx/2,self.Lx/2)
axval.set_ylim(-self.Ly/2,self.Ly/2)
elif self.conf.datatype=='experiment':
xmin=np.amin(self.conf.x)-20
ymin=np.amin(self.conf.y)-20
axval.set_xlim(xmin,xmin+self.Lx+120)
axval.set_ylim(ymin,ymin+self.Ly+120)
# returning figure pointer for use outside (like plotting or saving)
if 'figure' in kwargs:
return fig
elif 'axis' in kwargs:
return axval
else:
return fig
def draw_shm(shm, iteration=None):
"""
:param shm: System Health Map
:return:
"""
print("===========================================")
print("GENERATING SYSTEM HEALTH MAP DRAWING...")
x_size = float(Config.ag.x_size)
y_size = float(Config.ag.y_size)
z_size = float(Config.ag.z_size)
fig = plt.figure(figsize=(10 * x_size, 10 * y_size))
if z_size == 1:
plt.ylim([0, 1])
plt.xlim([0, 1])
else:
plt.ylim([0, z_size])
plt.xlim([0, z_size])
for node in shm.nodes():
location = AG_Functions.return_node_location(node)
x = (location[0] / x_size) * z_size
y = (location[1] / y_size) * z_size
z = (location[2] / z_size)
x_offset = z
y_offset = z
font_size = 35 / z_size
plt.text(x + 0.155 + x_offset,
y + 0.055 + y_offset,
str(node),
fontsize=font_size)
circle_router = plt.Circle((x + 0.1 + x_offset, y + 0.1 + y_offset),
0.05,
facecolor='w')
plt.gca().add_patch(circle_router)
if shm.node[node]['NodeHealth']:
color = 'w'
else:
color = 'r'
circle_node = plt.Circle((x + 0.14 + x_offset, y + 0.06 + y_offset),
0.01,
facecolor=color)
plt.gca().add_patch(circle_node)
for turn in shm.node[node]['TurnsHealth']:
if shm.node[node]['TurnsHealth'][turn]:
color = 'black'
else:
color = 'r'
if turn == 'S2E':
plt.gca().add_patch(
patches.Arrow(x + 0.11 + x_offset,
y + 0.065 + y_offset,
0.015,
0.015,
width=0.01,
color=color))
elif turn == 'E2S':
plt.gca().add_patch(
patches.Arrow(x + 0.135 + x_offset,
y + 0.08 + y_offset,
-0.015,
-0.015,
width=0.01,
color=color))
elif turn == 'W2N':
plt.gca().add_patch(
patches.Arrow(x + 0.065 + x_offset,
y + 0.117 + y_offset,
0.015,
0.015,
width=0.01,
color=color))
elif turn == 'N2W':
plt.gca().add_patch(
patches.Arrow(x + 0.09 + x_offset,
y + 0.132 + y_offset,
-0.015,
-0.015,
width=0.01,
color=color))
elif turn == 'N2E':
plt.gca().add_patch(
patches.Arrow(x + 0.12 + x_offset,
y + 0.132 + y_offset,
0.015,
-0.015,
width=0.01,
color=color))
elif turn == 'E2N':
plt.gca().add_patch(
patches.Arrow(x + 0.125 + x_offset,
y + 0.117 + y_offset,
-0.015,
0.015,
width=0.01,
color=color))
elif turn == 'W2S':
plt.gca().add_patch(
patches.Arrow(x + 0.075 + x_offset,
y + 0.08 + y_offset,
0.015,
-0.015,
width=0.01,
color=color))
elif turn == 'S2W':
plt.gca().add_patch(
patches.Arrow(x + 0.080 + x_offset,
y + 0.065 + y_offset,
-0.015,
0.015,
width=0.01,
color=color))
if shm.node[node]['TurnsHealth'][turn]:
color = 'black'
else:
color = 'r'
if turn == 'N2U':
circle_node = plt.Circle(
(x + 0.09 + x_offset, y + 0.142 + y_offset),
0.005,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
elif turn == 'N2D':
circle_node = plt.Circle(
(x + 0.11 + x_offset, y + 0.142 + y_offset),
0.005,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
circle_node = plt.Circle(
(x + 0.11 + x_offset, y + 0.142 + y_offset),
0.001,
facecolor='b')
plt.gca().add_patch(circle_node)
elif turn == 'S2U':
circle_node = plt.Circle(
(x + 0.11 + x_offset, y + 0.057 + y_offset),
0.005,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
elif turn == 'S2D':
circle_node = plt.Circle(
(x + 0.09 + x_offset, y + 0.057 + y_offset),
0.005,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
circle_node = plt.Circle(
(x + 0.09 + x_offset, y + 0.057 + y_offset),
0.001,
facecolor='b')
plt.gca().add_patch(circle_node)
elif turn == 'E2U':
circle_node = plt.Circle(
(x + 0.142 + x_offset, y + 0.11 + y_offset),
0.005,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
elif turn == 'E2D':
circle_node = plt.Circle(
(x + 0.142 + x_offset, y + 0.09 + y_offset),
0.005,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
circle_node = plt.Circle(
(x + 0.142 + x_offset, y + 0.09 + y_offset),
0.001,
facecolor='b')
plt.gca().add_patch(circle_node)
elif turn == 'W2U':
circle_node = plt.Circle(
(x + 0.057 + x_offset, y + 0.09 + y_offset),
0.005,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
elif turn == 'W2D':
circle_node = plt.Circle(
(x + 0.057 + x_offset, y + 0.11 + y_offset),
0.005,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
circle_node = plt.Circle(
(x + 0.057 + x_offset, y + 0.11 + y_offset),
0.001,
facecolor='b')
plt.gca().add_patch(circle_node)
elif turn == 'U2N':
circle_node = plt.Circle(
(x + 0.105 + x_offset, y + 0.111 + y_offset),
0.005,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
circle_node = plt.Circle(
(x + 0.105 + x_offset, y + 0.111 + y_offset),
0.001,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
plt.gca().add_patch(
patches.Arrow(x + 0.105 + x_offset,
y + 0.116 + y_offset,
0,
0.01,
width=0.01,
color=color))
elif turn == 'U2S':
circle_node = plt.Circle(
(x + 0.105 + x_offset, y + 0.086 + y_offset),
0.005,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
circle_node = plt.Circle(
(x + 0.105 + x_offset, y + 0.086 + y_offset),
0.001,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
plt.gca().add_patch(
patches.Arrow(x + 0.105 + x_offset,
y + 0.081 + y_offset,
0,
-0.01,
width=0.01,
color=color))
elif turn == 'U2W':
circle_node = plt.Circle(
(x + 0.085 + x_offset, y + 0.093 + y_offset),
0.005,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
circle_node = plt.Circle(
(x + 0.085 + x_offset, y + 0.093 + y_offset),
0.001,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
plt.gca().add_patch(
patches.Arrow(x + 0.08 + x_offset,
y + 0.093 + y_offset,
-0.01,
0,
width=0.01,
color=color))
elif turn == 'U2E':
circle_node = plt.Circle(
(x + 0.115 + x_offset, y + 0.093 + y_offset),
0.005,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
circle_node = plt.Circle(
(x + 0.115 + x_offset, y + 0.093 + y_offset),
0.001,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
plt.gca().add_patch(
patches.Arrow(x + 0.12 + x_offset,
y + 0.093 + y_offset,
0.01,
0,
width=0.01,
color=color))
elif turn == 'D2N':
circle_node = plt.Circle(
(x + 0.095 + x_offset, y + 0.111 + y_offset),
0.005,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
plt.gca().add_patch(
patches.Arrow(x + 0.095 + x_offset,
y + 0.116 + y_offset,
0,
0.01,
width=0.01,
color=color))
elif turn == 'D2S':
circle_node = plt.Circle(
(x + 0.095 + x_offset, y + 0.086 + y_offset),
0.005,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
plt.gca().add_patch(
patches.Arrow(x + 0.095 + x_offset,
y + 0.081 + y_offset,
0,
-0.01,
width=0.01,
color=color))
elif turn == 'D2W':
circle_node = plt.Circle(
(x + 0.085 + x_offset, y + 0.104 + y_offset),
0.005,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
plt.gca().add_patch(
patches.Arrow(x + 0.08 + x_offset,
y + 0.104 + y_offset,
-0.01,
0,
width=0.01,
color=color))
elif turn == 'D2E':
circle_node = plt.Circle(
(x + 0.115 + x_offset, y + 0.104 + y_offset),
0.005,
edgecolor=color,
facecolor='w')
plt.gca().add_patch(circle_node)
plt.gca().add_patch(
patches.Arrow(x + 0.12 + x_offset,
y + 0.104 + y_offset,
0.01,
0,
width=0.01,
color=color))
for link in shm.edges():
if shm.edges[link]['LinkHealth']:
color = 'black'
else:
color = 'r'
source_loc = AG_Functions.return_node_location(link[0])
destination_loc = AG_Functions.return_node_location(link[1])
x = (source_loc[0] / x_size) * z_size
y = (source_loc[1] / y_size) * z_size
z = source_loc[2] / z_size
x_offset = z
y_offset = z
dx = ((destination_loc[0] - source_loc[0]) / x_size)
dy = ((destination_loc[1] - source_loc[1]) / y_size)
dz = ((destination_loc[2] - source_loc[2]) / z_size)
if dz == 0:
if dx == 0:
if dy > 0:
plt.gca().add_patch(
patches.Arrow(x + 0.11 + x_offset,
y + 0.15 + y_offset,
0,
dy * z_size - 0.1,
width=0.01,
color=color))
else:
plt.gca().add_patch(
patches.Arrow(x + 0.09 + x_offset,
y + 0.05 + y_offset,
0,
dy * z_size + 0.1,
width=0.01,
color=color))
elif dy == 0:
if dx > 0:
plt.gca().add_patch(
patches.Arrow(x + 0.15 + x_offset,
y + 0.11 + y_offset,
dx * z_size - 0.1,
0,
width=0.01,
color=color))
else:
plt.gca().add_patch(
patches.Arrow(x + 0.05 + x_offset,
y + 0.09 + y_offset,
dx * z_size + 0.1,
0,
width=0.01,
color=color))
else:
raise ValueError("Can not draw link", link)
elif dz > 0:
z_offset = 1.4 / z_size
plt.gca().add_patch(
patches.Arrow(x + 0.130 + x_offset,
y + 0.140 + y_offset,
dz * z_offset,
dz * z_offset,
width=0.01,
color=color))
elif dz < 0:
plt.gca().add_patch(
patches.Arrow(x + 0.07 + x_offset,
y + 0.06 + y_offset,
dz * z_offset,
dz * z_offset,
width=0.01,
color=color))
fig.text(0.25, 0.02, "System Health Map", fontsize=35)
if iteration is None:
plt.savefig("GraphDrawings/SHM.png", dpi=200)
else:
plt.savefig("GraphDrawings/SHM_" + str(iteration) + ".png", dpi=200)
plt.clf()
plt.close(fig)
print(
"\033[35m* VIZ::\033[0mSYSTEM HEALTH MAP DRAWING CREATED AT: GraphDrawings/SHM.png"
)
return None
arrow = mpatches.FancyArrowPatch((x_tail, y_tail), (dx, dy),
mutation_scale=100,
transform=axs[1].transAxes)
axs[1].add_patch(arrow)
axs[1].set_xlim(0, 2)
axs[1].set_ylim(0, 2)
###############################################################################
# Head shape and anchor points fixed in data space
# ------------------------------------------------
#
# In this case we use `.patches.Arrow`
#
# Note that when the axis limits are changed, the arrow shape and location
# changes.
fig, axs = plt.subplots(nrows=2)
arrow = mpatches.Arrow(x_tail, y_tail, dx, dy)
axs[0].add_patch(arrow)
arrow = mpatches.Arrow(x_tail, y_tail, dx, dy)
axs[1].add_patch(arrow)
axs[1].set_xlim(0, 2)
axs[1].set_ylim(0, 2)
###############################################################################
plt.show()
def _goal_vec(self):
data = self.obs[self.obs_index_dict['goal_vec']]
self.goal_vec_arrow_handle.remove()
self.goal_vec_arrow = patches.Arrow(x=0, y=0, dx=data[0], dy=data[1])
self.goal_vec_arrow_handle = self.ax_dict['goal_vec'].add_artist(self.goal_vec_arrow)
def draw_networkx_edges_with_arrows(G,
pos,
width,
edge_color,
plt,
alpha=0.5,
ax=None):
ec = colorConverter.to_rgba(edge_color, alpha)
if ax is None:
ax = plt.gca()
edge_pos = np.asarray([(pos[e[0]], pos[e[1]]) for e in G.edges()])
edge_collection = LineCollection(edge_pos,
colors=ec,
linewidths=width,
antialiaseds=(1, ),
linestyle='solid',
transOffset=ax.transData)
edge_collection.set_zorder(1)
ax.add_collection(edge_collection)
edge_collection.set_alpha(alpha)
p = .8 # length of edge apart from the arrow part
for (src, dst), lwi in zip(edge_pos, width):
x1, y1 = src
x2, y2 = dst
dx = x2 - x1 # x offset
dy = y2 - y1 # y offset
d = np.sqrt(float(dx**2 + dy**2)) # length of edge
if d == 0:
continue
if dx == 0: # vertical edge
xa = x2
ya = dy * p + y1
if dy == 0: # horizontal edge
ya = y2
xa = dx * p + x1
else:
theta = np.arctan2(dy, dx)
xa = p * d * np.cos(theta) + x1
ya = p * d * np.sin(theta) + y1
dx, dy = x2 - xa, y2 - ya
patch = mpatches.Arrow(xa,
ya,
dx,
dy,
width=lwi / 55,
alpha=lwi * alpha / 5,
color=ec,
transform=ax.transData)
ax.add_patch(patch)
minx = np.amin(np.ravel(edge_pos[:, :, 0]))
maxx = np.amax(np.ravel(edge_pos[:, :, 0]))
miny = np.amin(np.ravel(edge_pos[:, :, 1]))
maxy = np.amax(np.ravel(edge_pos[:, :, 1]))
w = maxx - minx
h = maxy - miny
padx, pady = 0.05 * w, 0.05 * h
corners = (minx - padx, miny - pady), (maxx + padx, maxy + pady)
ax.update_datalim(corners)
ax.autoscale_view()
return edge_collection
def artist_reference():
import matplotlib.path as mpath
import matplotlib.lines as mlines
import matplotlib.patches as mpatches
from matplotlib.collections import PatchCollection
def label(xy, text):
y = xy[
1] - 0.15 # shift y-value for label so that it's below the artist
plt.text(xy[0], y, text, ha="center", family='sans-serif',
size=14) # ha = horizontalalignment
fig, ax = plt.subplots()
grid = np.mgrid[0.2:0.8:0.3, 0.2:0.8:0.3].reshape(2, -1).T
patches = []
# add a circle
circle = mpatches.Circle(grid[0], 0.1, ec="none")
patches.append(circle)
label(grid[0], "Circle")
# add a rectangle
rect = mpatches.Rectangle(grid[1] - [0.025, 0.05], 0.05, 0.1, ec="none")
patches.append(rect)
label(grid[1], "Rectangle")
# add a wedge
wedge = mpatches.Wedge(grid[2], 0.1, 30, 270, ec="none")
patches.append(wedge)
label(grid[2], "Wedge")
# add a Polygon
polygon = mpatches.RegularPolygon(grid[3], 5, 0.1)
patches.append(polygon)
label(grid[3], "Polygon")
# add an ellipse
ellipse = mpatches.Ellipse(grid[4], 0.2, 0.1)
patches.append(ellipse)
label(grid[4], "Ellipse")
# add an arrow
arrow = mpatches.Arrow(grid[5, 0] - 0.05,
grid[5, 1] - 0.05,
0.1,
0.1,
width=0.1)
patches.append(arrow)
label(grid[5], "Arrow")
# add a path patch
Path = mpath.Path
path_data = [(Path.MOVETO, [0.018, -0.11]), (Path.CURVE4, [-0.031,
-0.051]),
(Path.CURVE4, [-0.115, 0.073]), (Path.CURVE4, [-0.03, 0.073]),
(Path.LINETO, [-0.011, 0.039]), (Path.CURVE4, [0.043, 0.121]),
(Path.CURVE4, [0.075, -0.005]), (Path.CURVE4, [0.035,
-0.027]),
(Path.CLOSEPOLY, [0.018, -0.11])]
codes, verts = zip(*path_data)
path = mpath.Path(verts + grid[6], codes)
patch = mpatches.PathPatch(path)
patches.append(patch)
label(grid[6], "PathPatch")
# add a fancy box
fancybox = mpatches.FancyBboxPatch(grid[7] - [0.025, 0.05],
0.05,
0.1,
boxstyle=mpatches.BoxStyle("Round",
pad=0.02))
patches.append(fancybox)
label(grid[7], "FancyBboxPatch")
# add a line
x, y = np.array([[-0.06, 0.0, 0.1], [0.05, -0.05, 0.05]])
line = mlines.Line2D(x + grid[8, 0], y + grid[8, 1], lw=5., alpha=0.3)
label(grid[8], "Line2D")
colors = np.linspace(0, 1, len(patches))
collection = PatchCollection(patches, cmap=plt.cm.hsv, alpha=0.3)
collection.set_array(np.array(colors))
ax.add_collection(collection)
ax.add_line(line)
plt.subplots_adjust(left=0, right=1, bottom=0, top=1)
plt.axis('equal')
plt.axis('off')
plt.show()
pass
random_state = np.random.RandomState(0)
clf = RandomForestClassifier(random_state=random_state)
cv = StratifiedKFold(n_splits=5, shuffle=False)
# * ROC is receiver operationg characteristic. In this curve x axis is false positive rate and y axis is true positive rate
# * If the curve in plot is closer to left-top corner, test is more accurate.
# * Roc curve score is auc that is computation area under the curve from prediction scores
# * We want auc to closer 1
# In[5]:
# plot arrows
fig1 = plt.figure(figsize=[12, 12])
ax1 = fig1.add_subplot(111, aspect='equal')
ax1.add_patch(
patches.Arrow(0.45, 0.5, -0.25, 0.25, width=0.3, color='green', alpha=0.5))
ax1.add_patch(
patches.Arrow(0.5, 0.45, 0.25, -0.25, width=0.3, color='red', alpha=0.5))
tprs = []
aucs = []
mean_fpr = np.linspace(0, 1, 100)
i = 1
for train, test in cv.split(x, y):
prediction = clf.fit(x.iloc[train], y.iloc[train]).predict(x.iloc[test])
fpr, tpr, t = roc_curve(y[test], prediction)
tprs.append(interp(mean_fpr, fpr, tpr))
roc_auc = auc(fpr, tpr)
aucs.append(roc_auc)
plt.plot(fpr,
tpr,
