Skip to content

Commit acb2436

Browse files
JRicardoSanJRicardoSan
committed
Making a test with a crater scene
1 parent f655155 commit acb2436

24 files changed

Lines changed: 1420 additions & 172 deletions

‎camis/anisotropic_path_planning.py‎

Lines changed: 103 additions & 60 deletions
Large diffs are not rendered by default.

‎camis/cost_function.py‎

Lines changed: 36 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -771,10 +771,11 @@ def getCl(self, steepness_deg):
771771
# return np.max((self.getCd(steepness_deg), rawC * W)) # TODO: make this configurable?
772772

773773
def getCn(self,slope): #TODO - check this
774-
cd = self.getCd(slope)
775-
ca = self.getCa(slope)
776-
cl = self.getCl(slope)
777-
return (ca + cd)/2*np.sqrt(cl/np.sqrt(ca*cd))
774+
return self.getCa(slope)
775+
# cd = self.getCd(slope)
776+
# ca = self.getCa(slope)
777+
# cl = self.getCl(slope)
778+
# return (ca + cd)/2*np.sqrt(cl/np.sqrt(ca*cd))
778779

779780
def computeAniCoLUT(self):
780781
linearGradient = np.linspace(0,89,90)
@@ -999,16 +1000,18 @@ def showCAMIS(self,limAngle):
9991000
heading = np.arange(0, 2*np.pi, 0.01)
10001001
aspect = [1,0]
10011002

1002-
fig = plt.figure()
1003+
fig = plt.figure(figsize=(14.0, 3.5))
10031004

1004-
axes3 = fig.add_subplot(221, projection='3d')
1005+
axes3 = fig.add_subplot(131, projection='3d')
10051006
Xs = []
10061007
Ys = []
10071008

10081009
Cd = np.zeros_like(linearGradient)
10091010
Ca = np.zeros_like(linearGradient)
10101011
Cl1 = np.zeros_like(linearGradient)
10111012
Cl2 = np.zeros_like(linearGradient)
1013+
1014+
lastCa = []
10121015

10131016
for i,g in enumerate(linearGradient):
10141017
Cd[i] = np.min((self.limit_cost, self.getCd(linearGradient[i])))
@@ -1020,16 +1023,17 @@ def showCAMIS(self,limAngle):
10201023
preCost = computeCAMIScost(B,Cd[i],Ca[i],Cl1[i],Cl2[i])
10211024
Xs.append(B[0]*preCost)
10221025
Ys.append(B[1]*preCost)
1023-
axes3.plot(Xs, Ys, g, color=plt.cm.jet(float(g)/self.limit_angle_deg))
1026+
axes3.plot(Xs, Ys, g, color=plt.cm.jet(float(g)/limAngle))
10241027
Xs = []
10251028
Ys = []
1029+
lastCa = max(Ca[i],Cl1[i])
10261030

1027-
axes3.set_xlim(-self.limit_cost,self.limit_cost)
1028-
axes3.set_ylim(-self.limit_cost,self.limit_cost)
1031+
axes3.set_xlim(-lastCa,lastCa)
1032+
axes3.set_ylim(-lastCa,lastCa)
10291033
axes3.set_zlim(0,1.2*linearGradient[-1])
1030-
axes3.set_xlabel('Parallel Cost',fontsize='medium')
1031-
axes3.set_ylabel('Perpendicular Cost',fontsize='medium')
1032-
axes3.set_zlabel('Steepness (deg)',fontsize='medium')
1034+
axes3.set_xlabel('Ascent-Descent Cost [As/m]',fontsize='medium')
1035+
axes3.set_ylabel('Lateral Cost [As/m]',fontsize='medium')
1036+
axes3.set_zlabel('Steepness $α_{ij}$ [deg]',fontsize='medium')
10331037
axes3.xaxis.labelpad=-12
10341038
axes3.yaxis.labelpad=-12
10351039
axes3.zaxis.labelpad=-14
@@ -1041,10 +1045,10 @@ def showCAMIS(self,limAngle):
10411045
for spine in axes3.spines.values():
10421046
spine.set_visible(False)
10431047

1044-
1045-
axes5 = plt.subplot(222, projection='polar')
1046-
axes6 = plt.subplot(224, projection='polar')
1047-
axes5.set_facecolor('xkcd:light blue')
1048+
axes3.set_facecolor('w')
1049+
# axes5 = plt.subplot(222, projection='polar')
1050+
axes6 = plt.subplot(132, projection='polar')
1051+
# axes5.set_facecolor('xkcd:light blue')
10481052
axes6.set_facecolor('xkcd:light sage')
10491053
Bs = []
10501054
Ps = []
@@ -1056,14 +1060,14 @@ def showCAMIS(self,limAngle):
10561060
preCost = computeCAMIScost(B,Cd[i],Ca[i],Cl1[i],Cl2[i])
10571061
Cs.append(preCost)
10581062
Ps.append(1/preCost)
1059-
axes5.plot(Bs, Cs, 'xkcd:sea blue', lw = 2, color = plt.cm.jet(float(g)/self.limit_angle_deg))
1060-
axes6.plot(Bs, Ps, 'xkcd:leaf', lw = 2, color = plt.cm.jet(float(g)/self.limit_angle_deg))
1063+
# axes5.plot(Bs, Cs, 'xkcd:sea blue', lw = 2, color = plt.cm.jet(float(g)/self.limit_angle_deg))
1064+
axes6.plot(Bs, Ps, 'xkcd:leaf', lw = 2, color = plt.cm.jet(float(g)/limAngle))
10611065
Cs = []
10621066
Ps = []
10631067
Bs = []
10641068
fig.tight_layout()
10651069

1066-
ax3 = fig.add_subplot(223)
1070+
ax3 = fig.add_subplot(133)
10671071
sqrtQ1 = np.zeros_like(linearGradient)
10681072
sqrtQ2 = np.zeros_like(linearGradient)
10691073
D1 = np.zeros_like(linearGradient)
@@ -1075,13 +1079,13 @@ def showCAMIS(self,limAngle):
10751079
D1[i] = (Ca[i]-Cd[i])/2
10761080
D2[i] = (Cl2[i]-Cl1[i])/2
10771081
# Cn[i] = math.sqrt((Ca[i]+Cd[i])*(Cl1[i]+Cl2[i])/4)
1078-
l1 = ax3.plot(linearGradient,sqrtQ1,color='b', label = '$K_∥$')
1079-
l2 = ax3.plot(linearGradient,sqrtQ2,color='g', label = '$K_⊥$')
1080-
l3 = ax3.plot(linearGradient,D1,color='m', label = '$D_∥$')
1082+
# l1 = ax3.plot(linearGradient,sqrtQ1,color='b', label = '$K_∥$')
1083+
# l2 = ax3.plot(linearGradient,sqrtQ2,color='g', label = '$K_⊥$')
1084+
# l3 = ax3.plot(linearGradient,D1,color='m', label = '$D_∥$')
10811085
#l4 = ax3.plot(linearGradient,D2,color='y', label = '$D_⊥$')
1082-
l5 = ax3.plot(linearGradient,Ca,color='b', linestyle='dashed', label = '$C_a$')
1083-
l6 = ax3.plot(linearGradient,Cd,color='g', linestyle='dashed', label = '$C_d$')
1084-
l7 = ax3.plot(linearGradient,Cl1,color='m', linestyle='dashed', label = '$C_{l}$')
1086+
l5 = ax3.plot(linearGradient,Ca,color='b', linestyle='dashed', label = '$C^a(x_{ij})$')
1087+
l6 = ax3.plot(linearGradient,Cd,color='g', linestyle='dashed', label = '$C^d(x_{ij})$')
1088+
l7 = ax3.plot(linearGradient,Cl1,color='m', linestyle='dashed', label = '$C^l(x_{ij})$')
10851089
#l8 = ax3.plot(linearGradient,Cl2,color='y', linestyle='dashed', label = '$C_{l2}$')
10861090
# box = ax3.get_position()
10871091
# ax3.set_position([box.x0, box.y0, box.width * 0.5, box.height])
@@ -1104,13 +1108,14 @@ def showCAMIS(self,limAngle):
11041108
Cs = []
11051109
Bs = []
11061110
ax3b = ax3.twinx()
1107-
l9 = ax3b.plot(linearGradient,Anisotropy,color='r', label = '$ϒ$')
1108-
l10 = ax3b.plot(linearGradient,Cn,color='r', linestyle='dashed', label = '$C_n/C_o$')
1109-
ax3b.set_ylabel('Ratio', color='r')
1111+
l9 = ax3b.plot(linearGradient,Anisotropy,color='r', label = '$ϒ(x_{ij})$')
1112+
# l10 = ax3b.plot(linearGradient,Cn,color='r', linestyle='dashed', label = '$C_n/C_o$')
1113+
ax3b.set_ylabel('Anisotropy Ratio', color='r')
11101114
ax3b.yaxis.set_major_formatter(FormatStrFormatter('%.1f'))
11111115
ax3b.tick_params('y', colors='r')
1112-
lns = l1+l2+l3+l5+l6 + l7 + l9 + l10
1116+
lns = l5+l6 + l7 + l9
11131117
labs = [l.get_label() for l in lns]
1114-
ax3.legend(lns, labs, fontsize='small',\
1115-
loc='upper right', bbox_to_anchor=(1.5, 1.1), ncol=1)
1118+
# ax3.legend(lns, labs, fontsize='small',\
1119+
# loc='upper right', bbox_to_anchor=(1.5, 1.1), ncol=1)
1120+
ax3.legend(lns, labs, fontsize='large', ncol=2)
11161121
fig.tight_layout()

‎camis/cost_mapping.py‎

Lines changed: 112 additions & 55 deletions
Original file line numberDiff line numberDiff line change
@@ -473,7 +473,8 @@ def executePlanning(self, goal, start):
473473
return elapsedTime
474474

475475
# Executing BiOUM to compute the path faster
476-
def executeSqBiPlanning(self, goal, start, nbUpdate, anisoSearch):
476+
def executeSqBiPlanning(self, goal, start, nbUpdate, anisoSearch,
477+
dirPolicy):
477478
init = time()
478479
ijStart = np.round([(start[0] -
479480
self.sqXmap[0][0])/self.sqCellLateral,(start[1] -
@@ -492,31 +493,43 @@ def executeSqBiPlanning(self, goal, start, nbUpdate, anisoSearch):
492493
ap.computeBiTmap(self.sqVCMap, self.sqAspectMap,
493494
self.sqAnisotropyMap, ijGoal, ijStart, self.sqXmap,
494495
self.sqYmap, self.sqCellLateral, gridtype='sq',
495-
nbUpdate = nbUpdate, anisoSearch = anisoSearch)
496+
nbUpdate = nbUpdate, anisoSearch = anisoSearch,
497+
dirPolicy = dirPolicy)
496498

497499
print('Elapsed time to compute the Total Cost Map: ' +
498500
str(time()-init) + ' seconds')
499-
TlinkG = self.sqTmapG[self.sqLinkNode[1],self.sqLinkNode[0]]
500-
TlinkS = self.sqTmapS[self.sqLinkNode[1],self.sqLinkNode[0]]
501-
self.sqTmap = np.zeros_like(self.sqTmapG)
502-
self.sqTmap[:] = self.sqTmapG
503-
self.sqTmap[np.where(self.sqTmapS != np.inf)] = TlinkS + TlinkG - self.sqTmapS[np.where(self.sqTmapS != np.inf)]
504-
self.sqTmap[np.where(self.sqTmap < 0.0)] = np.inf
505-
506-
self.sqLinkPos = np.empty([2],dtype=float)
507-
self.sqLinkPos[:] = [self.sqXmap[self.sqLinkNode[1],
508-
self.sqLinkNode[0]],
509-
self.sqYmap[self.sqLinkNode[1],
510-
self.sqLinkNode[0]]]
511-
512-
self.sqPathG,uu = ap.getSqPath(self.sqDirMapG, self.sqLinkPos, goal,
513-
self.xMin, self.yMin, self.sqCellLateral)
514-
self.sqPathS,uu = ap.getSqPath(self.sqDirMapS, self.sqLinkPos, start,
515-
self.xMin, self.yMin, self.sqCellLateral)
516-
self.sqPath = np.concatenate((np.flipud(np.asarray(self.sqPathS)),
517-
np.asarray(self.sqPathG)))
518-
519-
def executeHexBiPlanning(self, goal, start, nbUpdate, anisoSearch):
501+
if dirPolicy == 'bidir':
502+
TlinkG = self.sqTmapG[self.sqLinkNode[1],self.sqLinkNode[0]]
503+
TlinkS = self.sqTmapS[self.sqLinkNode[1],self.sqLinkNode[0]]
504+
self.sqTmap = np.zeros_like(self.sqTmapG)
505+
self.sqTmap[:] = self.sqTmapG
506+
self.sqTmap[np.where(self.sqTmapS != np.inf)] = TlinkS + TlinkG - self.sqTmapS[np.where(self.sqTmapS != np.inf)]
507+
self.sqTmap[np.where(self.sqTmap < 0.0)] = np.inf
508+
509+
self.sqLinkPos = np.empty([2],dtype=float)
510+
self.sqLinkPos[:] = [self.sqXmap[self.sqLinkNode[1],
511+
self.sqLinkNode[0]],
512+
self.sqYmap[self.sqLinkNode[1],
513+
self.sqLinkNode[0]]]
514+
515+
self.sqPathG,uu = ap.getSqPath(self.sqDirMapG, self.sqLinkPos, goal,
516+
self.xMin, self.yMin, self.sqCellLateral)
517+
self.sqPathS,uu = ap.getSqPath(self.sqDirMapS, self.sqLinkPos, start,
518+
self.xMin, self.yMin, self.sqCellLateral)
519+
self.sqPath = np.concatenate((np.flipud(np.asarray(self.sqPathS)),
520+
np.asarray(self.sqPathG)))
521+
if dirPolicy == 'goal':
522+
self.sqTmap = self.sqTmapG
523+
path,uu = ap.getSqPath(self.sqDirMapG, start, goal,
524+
self.xMin, self.yMin,
525+
self.sqCellLateral)
526+
self.sqPath = np.asarray(path)
527+
# self.getSqPathData()
528+
# nbUpdate = True, False
529+
# anisoSearch = 'single', 'double'
530+
# dirPolicy = 'start','goal','bidir'
531+
def executeHexBiPlanning(self, goal, start, nbUpdate, anisoSearch,
532+
dirPolicy):
520533
init = time()
521534
IJ2XY = np.zeros([2,self.hexXmap.shape[0],self.hexYmap.shape[1]])
522535
IJ2XY[0] = self.hexXmap
@@ -533,31 +546,31 @@ def executeHexBiPlanning(self, goal, start, nbUpdate, anisoSearch):
533546
ap.computeBiTmap(self.hexVCMap, self.hexAspectMap,
534547
self.hexAnisotropyMap, ijGoal, ijStart, self.hexXmap,
535548
self.hexYmap, self.planRes, gridtype='hex',
536-
nbUpdate = nbUpdate, anisoSearch = anisoSearch)
549+
nbUpdate = nbUpdate, anisoSearch = anisoSearch,
550+
dirPolicy = dirPolicy)
537551
elapsedTime = time()-init
538552
print('Elapsed time to compute the Total Cost Map: '+str(elapsedTime))
539553
self.linkNode = nodeLink
540554
self.linkPos = IJ2XY[:,nodeLink[1],nodeLink[0]]
541-
542-
self.pathG,uu = ap.getPath(self.dirMapG, IJ2XY, XY2IJ, self.linkPos,
543-
goal, self.xMin, self.yMin, self.planRes)
544-
self.pathS,uu = ap.getPath(self.dirMapS, IJ2XY, XY2IJ, self.linkPos,
545-
start, self.xMin, self.yMin, self.planRes)
546-
547555
self.IJ2XY = IJ2XY
548556
self.XY2IJ = XY2IJ
557+
if dirPolicy == 'bidir':
558+
self.pathG,uu = ap.getPath(self.dirMapG, IJ2XY, XY2IJ, self.linkPos,
559+
goal, self.xMin, self.yMin, self.planRes)
560+
self.pathS,uu = ap.getPath(self.dirMapS, IJ2XY, XY2IJ, self.linkPos,
561+
start, self.xMin, self.yMin, self.planRes)
549562

550-
TlinkG = self.TmapG[nodeLink[1],nodeLink[0]]
551-
TlinkS = self.TmapS[nodeLink[1],nodeLink[0]]
563+
TlinkG = self.TmapG[nodeLink[1],nodeLink[0]]
564+
TlinkS = self.TmapS[nodeLink[1],nodeLink[0]]
552565

553-
self.pathS = self.pathS[1:-1]
554-
555-
self.path = np.concatenate((np.flipud(np.asarray(self.pathS)),
556-
np.asarray(self.pathG)))
557-
self.Tmap = np.zeros_like(self.TmapG)
558-
self.Tmap[:] = self.TmapG
559-
self.Tmap[np.where(self.TmapS != np.inf)] = TlinkS + TlinkG - self.TmapS[np.where(self.TmapS != np.inf)]
560-
self.Tmap[np.where(self.Tmap < 0.0)] = np.inf
566+
self.pathS = self.pathS[1:-1]
567+
568+
self.path = np.concatenate((np.flipud(np.asarray(self.pathS)),
569+
np.asarray(self.pathG)))
570+
self.Tmap = np.zeros_like(self.TmapG)
571+
self.Tmap[:] = self.TmapG
572+
self.Tmap[np.where(self.TmapS != np.inf)] = TlinkS + TlinkG - self.TmapS[np.where(self.TmapS != np.inf)]
573+
self.Tmap[np.where(self.Tmap < 0.0)] = np.inf
561574
self.getPathData()
562575
self.elapsedTime = elapsedTime
563576
return elapsedTime
@@ -595,6 +608,13 @@ def getSlope(self, xPos, yPos, heading):
595608
s = np.nan
596609
slope.append(s)
597610
return slope
611+
612+
def getSqPathData(self):
613+
pathElevation = []
614+
for index, waypoint in enumerate(self.sqPath):
615+
pathElevation.append(ap.interpolatePoint(
616+
waypoint/self.sqCellLateral,self.elevationMap))
617+
self.pathElevation = pathElevation
598618

599619
def getPathData(self):
600620
pathElevation = []
@@ -757,44 +777,81 @@ def showHexAnisotropyMap(self):
757777
cbar.set_label('Anisotropy')
758778
ax.set_aspect('equal')
759779
plt.show()
780+
781+
def showElevationMap(self,fontsize):
782+
fig, ax = plt.subplots(constrained_layout=True)
783+
cc = ax.contourf(self.xMap, self.yMap, self.elevationMap,
784+
50, cmap = cm.gist_earth, extend='both')
785+
ax.contour(self.xMap, self.yMap, self.hexElevationMap,
786+
20, colors = 'k', alpha=.3)
787+
# cc = ax.scatter(self.hexXmap, self.hexYmap,
788+
# c = self.hexElevationMap, cmap=cm.gist_earth,s=20)
789+
ax.set_xlim([self.xMap[0,2], self.xMap[-1,-4]])
790+
ax.set_ylim([self.yMap[0,0], self.yMap[-1,-1]])
791+
cbar = fig.colorbar(cc)
792+
cbar.set_label('Elevation [m]', fontsize = fontsize)
793+
ax.set_aspect('equal')
760794

761-
def showHexElevationMap(self):
762-
self.showNavElevationMap('hex')
795+
ax.set_xlabel('X-axis [m]', fontsize = fontsize)
796+
ax.set_ylabel('Y-axis [m]', fontsize = fontsize)
763797

764-
def showSqElevationMap(self):
765-
self.showNavElevationMap('sq')
798+
def showHexElevationMap(self, fontsize):
799+
self.showNavElevationMap('hex', fontsize)
800+
801+
def showSqElevationMap(self, fontsize):
802+
self.showNavElevationMap('sq', fontsize)
766803

767-
def showNavElevationMap(self, option):
804+
def showNavElevationMap(self, option, fontsize):
768805
fig, ax = plt.subplots(constrained_layout=True)
769806
if option == 'sq':
770807
cc = ax.scatter(self.sqXmap, self.sqYmap, c = self.sqElevationMap,
771808
cmap=cm.gist_earth,s=20)
772809
ax.set_xlim([self.xMap[0,0], self.xMap[-1,-1]])
773810
ax.set_ylim([self.yMap[0,0], self.yMap[-1,-1]])
774811
elif option == 'hex':
775-
cc = ax.scatter(self.hexXmap, self.hexYmap,
776-
c = self.hexElevationMap, cmap=cm.gist_earth,s=20)
812+
cc = ax.contourf(self.hexXmap, self.hexYmap, self.hexElevationMap,
813+
50, cmap = cm.gist_earth, extend='both')
814+
ax.contour(self.hexXmap, self.hexYmap, self.hexElevationMap,
815+
10, colors = 'k', alpha=.3)
816+
# cc = ax.scatter(self.hexXmap, self.hexYmap,
817+
# c = self.hexElevationMap, cmap=cm.gist_earth,s=20)
777818
ax.set_xlim([self.xMap[0,2], self.xMap[-1,-4]])
778819
ax.set_ylim([self.yMap[0,0], self.yMap[-1,-1]])
779820
cbar = fig.colorbar(cc)
780-
cbar.set_label('Elevation (m)')
821+
cbar.set_label('Elevation [m]', fontsize = fontsize)
781822
ax.set_aspect('equal')
782823

783-
ax.set_xlabel('X-axis [m]')
784-
ax.set_ylabel('Y-axis [m]')
824+
ax.set_xlabel('X-axis [m]', fontsize = fontsize)
825+
ax.set_ylabel('Y-axis [m]', fontsize = fontsize)
785826

786-
787-
def showHexSteepnessMap(self):
827+
def showSqSlopeGradientMap(self, fontsize, maxGradient = 0.0):
828+
fig, ax = plt.subplots(constrained_layout=True)
829+
cc = ax.scatter(self.sqXmap, self.sqYmap, c =
830+
self.rad2deg*self.sqSlopeMap,
831+
cmap="nipy_spectral",s=20)
832+
if maxGradient > 0.0:
833+
cc.set_clim(0,maxGradient)
834+
ax.set_aspect('equal')
835+
cbar = fig.colorbar(cc)
836+
cbar.set_label('Slope Gradient [deg]', fontsize = fontsize)
837+
ax.set_aspect('equal')
838+
ax.set_xlim([self.xMap[0,0], self.xMap[-1,-1]])
839+
ax.set_ylim([self.yMap[0,0], self.yMap[-1,-1]])
840+
ax.set_xlabel('X-axis [m]', fontsize = fontsize)
841+
ax.set_ylabel('Y-axis [m]', fontsize = fontsize)
842+
plt.show()
843+
844+
def showHexSteepnessMap(self, fontsize):
788845
fig, ax = plt.subplots(constrained_layout=True)
789846
cc = ax.scatter(self.hexXmap, self.hexYmap, c = self.rad2deg*self.hexSlopeMap, cmap="nipy_spectral",s=20)
790847
ax.set_aspect('equal')
791848
cbar = fig.colorbar(cc)
792-
cbar.set_label('Slope (deg)')
849+
cbar.set_label('Slope (deg)', fontsize = fontsize)
793850
ax.set_aspect('equal')
794851
ax.set_xlim([self.xMap[0,2], self.xMap[-1,-4]])
795852
ax.set_ylim([self.yMap[0,0], self.yMap[-1,-1]])
796-
ax.set_xlabel('X-axis [m]')
797-
ax.set_ylabel('Y-axis [m]')
853+
ax.set_xlabel('X-axis [m]', fontsize = fontsize)
854+
ax.set_ylabel('Y-axis [m]', fontsize = fontsize)
798855
plt.show()
799856

800857

0 commit comments

Comments
 (0)