def __init__(self, initialPos, simtype="UAS_ROTOR", vehicleID = 0,
fasttime = True, verbose=0,callsign = "SPEEDBIRD",
monitor="DAIDALUS",
daaConfig="data/DaidalusQuadConfig.txt"):
self.fasttime = fasttime
self.callsign = callsign
self.verbose = verbose
self.home_pos = [initialPos[0], initialPos[1], initialPos[2]]
self.traffic = []
if simtype == "UAM_VTOL":
self.ownship = VehicleSim(vehicleID,0.0,0.0,0.0,0.0,0.0,0.0)
else:
from quadsim import QuadSim
self.ownship = QuadSim()
self.vehicleID = vehicleID
self.Cog = Cognition(callsign)
self.Guidance = Guidance(GuidanceParam())
self.Geofence = GeofenceMonitor([3,2,2,20,20])
self.Trajectory = Trajectory(callsign)
self.tfMonitor = TrafficMonitor(callsign,daaConfig,False,monitor)
self.Merger = Merger(callsign,vehicleID)
# Aircraft data
self.flightplan1 = []
self.etaFP1 = False
self.etaFP2 = False
self.flightplan2 = []
self.controlInput = [0.0,0.0,0.0]
self.fenceList = []
self.guidanceMode = GuidanceMode.NOOP
# Merger
self.arrTime = None
self.logLatency = 0
self.prevLogUpdate = 0
self.mergerLog = LogData()
self.position = self.home_pos
self.velocity = [0.0,0.0,0.0]
self.trkgsvs = [0.0,0.0,0.0]
self.localPos = []
self.ownshipLog = {"t": [], "position": [], "velocityNED": [], "positionNED": [],
"trkbands": [], "gsbands": [], "altbands": [], "vsbands": [],
"localPlans": [], "localFences": [], "commandedVelocityNED": []}
self.trafficLog = {}
self.emergbreak = False
if self.fasttime:
self.currTime = 0
else:
self.currTime = time.time()
self.numSecPlan = 0
self.plans = []
self.fences = []
self.mergeFixes = []
self.localPlans = []
self.localFences= []
self.localMergeFixes = []
self.daa_radius = 0
self.startSent = False
self.nextWP1 = 1
self.nextWP2 = 1
self.numFences = 0
self.resSpeed = 0
self.defaultWPSpeed = 1
self.missionComplete = False
self.fphases = -1
self.land = False
self.activePlan = "Plan0"
self.windFrom = 0
self.windSpeed = 0
class Icarous():
def __init__(self, initialPos, simtype="UAS_ROTOR", vehicleID = 0,
fasttime = True, verbose=0,callsign = "SPEEDBIRD",
monitor="DAIDALUS",
daaConfig="data/DaidalusQuadConfig.txt"):
self.fasttime = fasttime
self.callsign = callsign
self.verbose = verbose
self.home_pos = [initialPos[0], initialPos[1], initialPos[2]]
self.traffic = []
if simtype == "UAM_VTOL":
self.ownship = VehicleSim(vehicleID,0.0,0.0,0.0,0.0,0.0,0.0)
else:
from quadsim import QuadSim
self.ownship = QuadSim()
self.vehicleID = vehicleID
self.Cog = Cognition(callsign)
self.Guidance = Guidance(GuidanceParam())
self.Geofence = GeofenceMonitor([3,2,2,20,20])
self.Trajectory = Trajectory(callsign)
self.tfMonitor = TrafficMonitor(callsign,daaConfig,False,monitor)
self.Merger = Merger(callsign,vehicleID)
# Aircraft data
self.flightplan1 = []
self.etaFP1 = False
self.etaFP2 = False
self.flightplan2 = []
self.controlInput = [0.0,0.0,0.0]
self.fenceList = []
self.guidanceMode = GuidanceMode.NOOP
# Merger
self.arrTime = None
self.logLatency = 0
self.prevLogUpdate = 0
self.mergerLog = LogData()
self.position = self.home_pos
self.velocity = [0.0,0.0,0.0]
self.trkgsvs = [0.0,0.0,0.0]
self.localPos = []
self.ownshipLog = {"t": [], "position": [], "velocityNED": [], "positionNED": [],
"trkbands": [], "gsbands": [], "altbands": [], "vsbands": [],
"localPlans": [], "localFences": [], "commandedVelocityNED": []}
self.trafficLog = {}
self.emergbreak = False
if self.fasttime:
self.currTime = 0
else:
self.currTime = time.time()
self.numSecPlan = 0
self.plans = []
self.fences = []
self.mergeFixes = []
self.localPlans = []
self.localFences= []
self.localMergeFixes = []
self.daa_radius = 0
self.startSent = False
self.nextWP1 = 1
self.nextWP2 = 1
self.numFences = 0
self.resSpeed = 0
self.defaultWPSpeed = 1
self.missionComplete = False
self.fphases = -1
self.land = False
self.activePlan = "Plan0"
self.windFrom = 0
self.windSpeed = 0
def setpos_uncertainty(self,xx,yy,zz,xy,yz,xz,coeff=0.8):
self.ownship.setpos_uncertainty(xx,yy,zz,xy,yz,xz,coeff)
def InputWind(self,windFrom,windSpeed):
self.windFrom = windFrom
self.windSpeed = windSpeed
def InputTraffic(self,idx,position,velocity):
if idx is not self.vehicleID:
trkgsvs = ConvertVnedToTrkGsVs(velocity[0],velocity[1],velocity[2])
self.tfMonitor.input_traffic(idx,position,trkgsvs,self.currTime)
self.Trajectory.InputTrafficData(idx,position,velocity)
self.RecordTraffic(idx, position, velocity, self.ConvertToLocalCoordinates(position))
def InputFlightplan(self,fp,scenarioTime,eta=False):
self.flightplan1 = fp
self.flightplan2 = []
self.etaFP1 = eta
self.plans.append(fp)
self.localPlans.append(self.GetLocalFlightPlan(fp))
self.Trajectory.InputFlightplan("Plan0",fp,eta)
self.Cog.InputFlightplanData("Plan0",scenarioTime,fp,eta)
self.Guidance.InputFlightplanData("Plan0",scenarioTime,fp,eta)
def InputFlightplanFromFile(self,filename,scenarioTime=0,eta=False):
wp, time, speed = ReadFlightplanFile(filename)
fp = []
if eta:
combine = time
else:
combine = speed
for w,i in zip(wp,combine):
if not eta and i < 0:
i = self.defaultWPSpeed
fp.append(w + [i])
self.InputFlightplan(fp,scenarioTime,eta)
def ConvertToLocalCoordinates(self,pos):
dh = lambda x, y: abs(distance(self.home_pos[0],self.home_pos[1],x,y))
if pos[1] > self.home_pos[1]:
sgnX = 1
else:
sgnX = -1
if pos[0] > self.home_pos[0]:
sgnY = 1
else:
sgnY = -1
dx = dh(self.home_pos[0],pos[1])
dy = dh(pos[0],self.home_pos[1])
return [dy*sgnY,dx*sgnX,pos[2]]
def GetLocalFlightPlan(self,fp):
local = []
for wp in fp:
local.append(self.ConvertToLocalCoordinates(wp))
return local
def InputGeofence(self,filename):
self.fenceList = Getfence(filename)
for fence in self.fenceList:
self.Geofence.InputData(fence)
self.Trajectory.InputGeofenceData(fence)
self.numFences += 1
localFence = []
gf = []
for vertex in fence['Vertices']:
localFence.append(self.ConvertToLocalCoordinates([*vertex,0]))
gf.append([*vertex,0])
self.localFences.append(localFence)
self.fences.append(gf)
def InputMergeFixes(self,filename):
wp, ind, _ = ReadFlightplanFile(filename)
self.localMergeFixes = list(map(self.ConvertToLocalCoordinates, wp))
self.mergeFixes = wp
self.Merger.SetIntersectionData(list(zip(ind,wp)))
def SetGeofenceParams(self,params):
gfparams = [params['LOOKAHEAD'],
params['HTHRESHOLD'],
params['VTHRESHOLD'],
params['HSTEPBACK'],
params['VSTEPBACK']]
self.Geofence.SetParameters(gfparams)
def SetTrajectoryParams(self,params):
# Astar params
enable3d = True if params['ASTAR_ENABLE3D'] == 1 else False
gridSize = params['ASTAR_GRIDSIZE']
resSpeed = params['ASTAR_RESSPEED']
lookahead= params['ASTAR_LOOKAHEAD']
daaconfig = ''
self.Trajectory.UpdateAstarParams(enable3d,gridSize,resSpeed,lookahead,daaconfig)
# RRT Params
Nstep = int(params['RRT_NITERATIONS'])
dt = params['RRT_DT']
Dt = int(params['RRT_MACROSTEPS'])
capR = params['RRT_CAPR']
resSpeed = params['RRT_RESSPEED']
self.Trajectory.UpdateRRTParams(resSpeed,Nstep,dt,Dt,capR,daaconfig)
def SetGuidanceParams(self,params):
guidParams = GuidanceParam()
guidParams.defaultWpSpeed = params['DEF_WP_SPEED']
guidParams.captureRadiusScaling = params['CAP_R_SCALING']
guidParams.guidanceRadiusScaling = params['GUID_R_SCALING']
guidParams.xtrkDev = params['XTRKDEV']
guidParams.climbFpAngle = params['CLIMB_ANGLE']
guidParams.climbAngleVRange = params['CLIMB_ANGLE_VR']
guidParams.climbAngleHRange = params['CLIMB_ANGLE_HR']
guidParams.climbRateGain = params['CLIMB_RATE_GAIN']
guidParams.maxClimbRate = params['MAX_CLIMB_RATE']
guidParams.minClimbRate = params['MIN_CLIMB_RATE']
guidParams.maxCap = params['MAX_CAP']
guidParams.minCap = params['MIN_CAP']
guidParams.maxSpeed = params['MAX_GS'] * 0.5
guidParams.minSpeed = params['MIN_GS'] * 0.5
guidParams.yawForward = True if params['YAW_FORWARD'] == 1 else False
self.defaultWPSpeed = guidParams.defaultWpSpeed
self.Guidance.SetGuidanceParams(guidParams)
def SetCognitionParams(self,params):
cogParams = CognitionParams()
cogParams.resolutionSpeed = params['RESSPEED']
cogParams.searchType =int(params['SEARCHALGORITHM'])
cogParams.resolutionType = int(params['RES_TYPE'])
cogParams.allowedXTrackDeviation = params['XTRKDEV']
cogParams.DTHR = params['DET_1_WCV_DTHR']/3
cogParams.ZTHR = params['DET_1_WCV_ZTHR']/3
self.Cog.InputParameters(cogParams)
self.resSpeed = params["RESSPEED"]
def SetTrafficParams(self,params):
paramString = "" \
+"lookahead_time="+str(params['LOOKAHEAD_TIME'])+ "[s];"\
+"left_hdir="+str(params['LEFT_TRK'])+ "[deg];"\
+"right_hdir="+str(params['RIGHT_TRK'])+ "[deg];"\
+"min_hs="+str(params['MIN_GS'])+ "[knot];"\
+"max_hs="+str(params['MAX_GS'])+ "[knot];"\
+"min_vs="+str(params['MIN_VS'])+ "[fpm];"\
+"max_vs="+str(params['MAX_VS'])+ "[fpm];"\
+"min_alt="+str(params['MIN_ALT'])+ "[ft];"\
+"max_alt="+str(params['MAX_ALT'])+ "[ft];"\
+"step_hdir="+str(params['TRK_STEP'])+ "[deg];"\
+"step_hs="+str(params['GS_STEP'])+ "[knot];"\
+"step_vs="+str(params['VS_STEP'])+ "[fpm];"\
+"step_alt="+str(params['ALT_STEP'])+ "[ft];"\
+"horizontal_accel="+str(params['HORIZONTAL_ACCL'])+ "[m/s^2];"\
+"vertical_accel="+str(params['VERTICAL_ACCL'])+ "[m/s^2];"\
+"turn_rate="+str(params['TURN_RATE'])+ "[deg/s];"\
+"bank_angle="+str(params['BANK_ANGLE'])+ "[deg];"\
+"vertical_rate="+str(params['VERTICAL_RATE'])+ "[fpm];"\
+"recovery_stability_time="+str(params['RECOV_STAB_TIME'])+ "[s];"\
+"min_horizontal_recovery="+str(params['MIN_HORIZ_RECOV'])+ "[ft];"\
+"min_vertical_recovery="+str(params['MIN_VERT_RECOV'])+ "[ft];"\
+"recovery_hdir="+( "true;" if params['RECOVERY_TRK'] == 1 else "false;" )\
+"recovery_hs="+( "true;" if params['RECOVERY_GS'] == 1 else "false;" )\
+"recovery_vs="+( "true;" if params['RECOVERY_VS'] == 1 else "false;" )\
+"recovery_alt="+( "true;" if params['RECOVERY_ALT'] == 1 else "false;" )\
+"ca_bands="+( "true;" if params['CA_BANDS'] == 1 else "false;" )\
+"ca_factor="+str(params['CA_FACTOR'])+ ";"\
+"horizontal_nmac="+str(params['HORIZONTAL_NMAC'])+ "[ft];"\
+"vertical_nmac="+str(params['VERTICAL_NMAC'])+ "[ft];"\
+"conflict_crit="+( "true;" if params['CONFLICT_CRIT'] == 1 else "false;" )\
+"recovery_crit="+( "true;" if params['RECOVERY_CRIT'] == 1 else "false;" )\
+"contour_thr="+str(params['CONTOUR_THR'])+ "[deg];"\
+"alerters = default;"\
+"default_alert_1_alerting_time="+str(params['AL_1_ALERT_T'])+ "[s];"\
+"default_alert_1_detector="+"det_1;"\
+"default_alert_1_early_alerting_time="+str(params['AL_1_E_ALERT_T'])+ "[s];"\
+"default_alert_1_region="+"NEAR;"\
+"default_alert_1_spread_alt="+str(params['AL_1_SPREAD_ALT'])+ "[ft];"\
+"default_alert_1_spread_hs="+str(params['AL_1_SPREAD_GS'])+ "[knot];"\
+"default_alert_1_spread_hdir="+str(params['AL_1_SPREAD_TRK'])+ "[deg];"\
+"default_alert_1_spread_vs="+str(params['AL_1_SPREAD_VS'])+ "[fpm];"\
+"default_det_1_WCV_DTHR="+str(params['DET_1_WCV_DTHR'])+ "[ft];"\
+"default_det_1_WCV_TCOA="+str(params['DET_1_WCV_TCOA'])+ "[s];"\
+"default_det_1_WCV_TTHR="+str(params['DET_1_WCV_TTHR'])+ "[s];"\
+"default_det_1_WCV_ZTHR="+str(params['DET_1_WCV_ZTHR'])+ "[ft];"\
+"default_load_core_detection_det_1="+"gov.nasa.larcfm.ACCoRD.WCV_TAUMOD;"
daa_log = True if params['LOGDAADATA'] == 1 else False
self.tfMonitor.SetParameters(paramString,daa_log)
self.Trajectory.UpdateDAAParams(paramString)
self.daa_radius = params['DET_1_WCV_DTHR']/3
def SetMergerParams(self,params):
self.Merger.SetVehicleConstraints(params["MIN_GS"]*0.5,
params["MAX_GS"]*0.5,
params["MAX_TURN_RADIUS"])
self.Merger.SetFixParams(params["MIN_SEP_TIME"],
params["COORD_ZONE"],
params["SCHEDULE_ZONE"],
params["ENTRY_RADIUS"],
params["CORRIDOR_WIDTH"])
def SetParameters(self,params):
self.params = params
self.SetGuidanceParams(params)
self.SetGeofenceParams(params)
self.SetTrajectoryParams(params)
self.SetCognitionParams(params)
self.SetTrafficParams(params)
self.SetMergerParams(params)
def SetParametersFromFile(self,filename):
params = LoadIcarousParams(filename)
self.SetParameters(params)
def RunOwnship(self):
self.ownship.inputNED(*self.controlInput)
self.ownship.step(windFrom=self.windFrom,windSpeed=self.windSpeed)
opos = self.ownship.getOutputPositionNED()
ovel = self.ownship.getOutputVelocityNED()
self.localPos = opos
(ogx, ogy) = gps_offset(self.home_pos[0], self.home_pos[1], opos[1], opos[0])
self.position = [ogx, ogy, opos[2]]
self.velocity = ovel
self.trkgsvs = ConvertVnedToTrkGsVs(ovel[0],ovel[1],-ovel[2])
self.trkband = None
self.gsband = None
self.altband = None
self.vsband = None
self.RecordOwnship()
def RunCognition(self):
self.Cog.InputVehicleState(self.position,self.trkgsvs,self.trkgsvs[0])
nextWP1 = self.nextWP1
if self.nextWP1 >= len(self.flightplan1):
nextWP1 = len(self.flightplan1) - 1
dist = distance(self.position[0],self.position[1],self.flightplan1[nextWP1][0],self.flightplan1[nextWP1][1])
if self.verbose > 1:
if self.activePlan == 'Plan0':
print("%s: %s, Distance to wp %d: %f" %(self.callsign,self.activePlan,self.nextWP1,dist))
else:
print("%s: %s, Distance to wp %d: %f" %(self.callsign,self.activePlan,self.nextWP2,dist))
self.fphase = self.Cog.RunFlightPhases(self.currTime)
if self.fphase == 8:
self.land = True
n, cmd = self.Cog.GetOutput()
if n > 0:
if cmd.commandType == CommandTypes.FP_CHANGE:
self.guidanceMode = GuidanceMode.FLIGHTPLAN
self.activePlan = cmd.commandU.fpChange.name.decode('utf-8')
nextWP = cmd.commandU.fpChange.wpIndex
if self.activePlan == "Plan0":
self.flightplan2 = []
self.nextWP1 = nextWP
else:
self.nextWP2 = nextWP
self.Guidance.SetGuidanceMode(self.guidanceMode,self.activePlan,nextWP)
if self.verbose > 0:
print(self.callsign, ": active plan = ",self.activePlan)
elif cmd.commandType == CommandTypes.P2P:
self.guidanceMode = GuidanceMode.POINT2POINT
point = cmd.commandU.p2pCommand.point
speed = cmd.commandU.p2pCommand.speed
fp = [(*self.position,0),(*point,speed)]
self.Guidance.InputFlightplanData("P2P",0,fp)
self.activePlan = "P2P"
self.nextWP2 = 1
self.Guidance.SetGuidanceMode(self.guidanceMode,self.activePlan,1)
if self.verbose > 0:
print(self.callsign, ": active plan = ",self.activePlan)
elif cmd.commandType == CommandTypes.VELOCITY:
self.guidanceMode = GuidanceMode.VECTOR
vn = cmd.commandU.velocityCommand.vn
ve = cmd.commandU.velocityCommand.ve
vu = cmd.commandU.velocityCommand.vu
trkGsVs = ConvertVnedToTrkGsVs(vn,ve,vu)
self.Guidance.InputVelocityCmd(trkGsVs)
self.Guidance.SetGuidanceMode(self.guidanceMode,"",0)
elif cmd.commandType == CommandTypes.TAKEOFF:
self.guidanceMode = GuidanceMode.TAKEOFF
elif cmd.commandType == CommandTypes.SPEED_CHANGE:
self.etaFP1 = False
speedChange = cmd.commandU.speedChange.speed
planID= cmd.commandU.speedChange.name.decode('utf-8')
nextWP = self.nextWP1 if planID == "Plan0" else self.nextWP2
self.Guidance.ChangeWaypointSpeed(planID,nextWP,speedChange,False)
elif cmd.commandType == CommandTypes.STATUS_MESSAGE:
if self.verbose > 0:
message = cmd.commandU.statusMessage.buffer
print(self.callsign,":",message.decode('utf-8'))
elif cmd.commandType == CommandTypes.FP_REQUEST:
self.flightplan2 = []
self.numSecPlan += 1
searchType = cmd.commandU.fpRequest.searchType
startPos = cmd.commandU.fpRequest.fromPosition
stopPos = cmd.commandU.fpRequest.toPosition
startVel = cmd.commandU.fpRequest.startVelocity
planID = cmd.commandU.fpRequest.name.decode('utf-8')
numWP = self.Trajectory.FindPath(
searchType,
planID,
startPos,
stopPos,
startVel)
if numWP > 0:
if self.verbose > 0:
print("%s : At %f s, Computed flightplan %s with %d waypoints" % (self.callsign,self.currTime,planID,numWP))
for i in range(numWP):
wp = self.Trajectory.GetWaypoint(planID,i)
self.flightplan2.append([wp[0],wp[1],wp[2],self.resSpeed])
self.Cog.InputFlightplanData(planID,0,self.flightplan2)
self.Guidance.InputFlightplanData(planID,0,self.flightplan2)
self.plans.append(self.flightplan2)
self.localPlans.append(self.GetLocalFlightPlan(self.flightplan2))
else:
if self.verbose > 0:
print(self.callsign,"Error finding path")
def RunGuidance(self):
if self.guidanceMode is GuidanceMode.NOOP:
return
if self.guidanceMode is GuidanceMode.TAKEOFF:
self.Cog.InputReachedWaypoint("Takeoff",0);
return
self.Guidance.SetAircraftState(self.position,self.trkgsvs)
self.Guidance.RunGuidance(self.currTime)
guidOutput = self.Guidance.GetOutput()
self.controlInput = ConvertTrkGsVsToVned(guidOutput.velCmd[0],
guidOutput.velCmd[1],
guidOutput.velCmd[2])
nextWP = guidOutput.nextWP
if self.guidanceMode is not GuidanceMode.VECTOR:
if self.activePlan == "Plan0":
if self.nextWP1 < nextWP:
self.Cog.InputReachedWaypoint("Plan0",nextWP-1)
self.nextWP1 = nextWP
if self.verbose > 0 and self.nextWP1 < len(self.flightplan1):
print("%s : Proceeding to waypoint %d on %s" % (self.callsign,nextWP,self.activePlan))
else:
if self.nextWP2 < nextWP:
self.nextWP2 = nextWP
self.Cog.InputReachedWaypoint(self.activePlan,nextWP-1)
if self.verbose > 0 and self.nextWP2 < len(self.flightplan2):
print("%s : Proceeding to waypoint %d on %s" % (self.callsign,nextWP,self.activePlan))
else:
pass
def RunGeofenceMonitor(self):
gfConflictData = GeofenceConflict()
self.Geofence.CheckViolation(self.position,*self.trkgsvs)
gfConflictData.numConflicts = self.Geofence.GetNumConflicts()
gfConflictData.numFences = self.numFences
for i in range(gfConflictData.numConflicts):
(ids,conflict,violation,recPos,ftype) = self.Geofence.GetConflict(i)
gfConflictData.conflictFenceIDs[i] = ids
gfConflictData.conflictTypes[i] = ftype
gfConflictData.recoveryPosition = recPos
for i,fp in enumerate(self.flightplan1):
wp = fp[:3]
check1 = self.Geofence.CheckViolation(wp,0,0,0)
check2 = self.Geofence.CheckWPFeasibility(self.position,wp)
gfConflictData.waypointConflict1[i] = check1
gfConflictData.directPathToWaypoint1[i] = check2
for i,fp in enumerate(self.flightplan2):
wp = fp[:3]
check1 = self.Geofence.CheckViolation(wp,0,0,0)
check2 = self.Geofence.CheckWPFeasibility(self.position,wp)
gfConflictData.waypointConflict2[i] = check1
gfConflictData.directPathToWaypoint2[i] = check2
self.Cog.InputGeofenceConflictData(gfConflictData)
def RunTrafficMonitor(self):
self.tfMonitor.monitor_traffic(self.position,self.trkgsvs,self.currTime)
trkband = self.tfMonitor.GetTrackBands()
gsband = self.tfMonitor.GetGSBands()
altband = self.tfMonitor.GetAltBands()
vsband = self.tfMonitor.GetVSBands()
trkband.time = self.currTime
gsband.time = self.currTime
altband.time = self.currTime
vsband.time = self.currTime
feasibility_cp = True
if self.nextWP1 < len(self.flightplan1):
C1 = self.Trajectory.ComputeClosesetPoint(self.flightplan1[self.nextWP1-1],self.flightplan1[self.nextWP1],self.position)
feasibility_cp = self.tfMonitor.monitor_wp_feasibility(C1,-1)
for i,fp in enumerate(self.flightplan1):
wp = fp[:3]
feasibility = self.tfMonitor.monitor_wp_feasibility(wp,-1)
feasibility &= self.tfMonitor.monitor_wp_feasibility(wp,self.resSpeed)
trkband.wpFeasibility1[i] = feasibility
trkband.fp1ClosestPointFeasible = feasibility_cp
gsband.wpFeasibility1[i] = feasibility
altband.wpFeasibility1[i] = feasibility
vsband.wpFeasibility1[i] = feasibility
for i,fp in enumerate(self.flightplan2):
wp = fp[:3]
feasibility = self.tfMonitor.monitor_wp_feasibility(wp,-1)
feasibility &= self.tfMonitor.monitor_wp_feasibility(wp,self.resSpeed)
trkband.wpFeasibility2[i] = feasibility
gsband.wpFeasibility2[i] = feasibility
altband.wpFeasibility2[i] = feasibility
vsband.wpFeasibility2[i] = feasibility
self.Cog.InputBands(trkband,gsband,altband,vsband)
filterinfnan = lambda x: "nan" if np.isnan(x) else "inf" if np.isinf(x) else x
getbandlog = lambda bands: {} if bands is None else {"conflict": bands.currentConflictBand,
"resup": filterinfnan(bands.resUp),
"resdown": filterinfnan(bands.resDown),
"numBands": bands.numBands,
"bandTypes": [bands.type[i] for i in range(20)],
"low": [bands.min[i] for i in range(20)],
"high": [bands.max[i] for i in range(20)]}
self.ownshipLog["trkbands"].append(getbandlog(trkband))
self.ownshipLog["gsbands"].append(getbandlog(gsband))
self.ownshipLog["altbands"].append(getbandlog(altband))
self.ownshipLog["vsbands"].append(getbandlog(vsband))
def RunMerger(self):
self.Merger.SetAircraftState(self.position,self.velocity)
if self.currTime - self.prevLogUpdate > self.logLatency:
self.Merger.SetMergerLog(self.mergerLog)
self.prevLogUpdate = self.currTime
mergingActive = self.Merger.Run(self.currTime)
self.Cog.InputMergeStatus(mergingActive)
outAvail, arrTime = self.Merger.GetArrivalTimes()
if outAvail:
self.arrTime = arrTime
if mergingActive == 3:
velCmd = self.Merger.GetVelocityCmd()
speedChange = velCmd[1]
nextWP = self.nextWP1
self.Guidance.ChangeWaypointSpeed("Plan0",nextWP,speedChange,False)
def InputMergeLogs(self,logs,delay):
self.mergerLog = logs
self.logLatency = delay
def CheckMissionComplete(self):
if self.land and self.fphase == 0:
self.missionComplete = True
return self.missionComplete
def RecordOwnship(self):
self.ownshipLog["t"].append(self.currTime)
self.ownshipLog["position"].append(self.position)
self.ownshipLog["velocityNED"].append(self.velocity)
self.ownshipLog["positionNED"].append(self.localPos)
self.ownshipLog["commandedVelocityNED"].append(self.controlInput)
def RecordTraffic(self, id, position, velocity, positionLoc):
if id not in self.trafficLog:
self.trafficLog[id] = {"t": [], "position": [], "velocityNED": [], "positionNED": []}
self.trafficLog[id]["t"].append(self.currTime)
self.trafficLog[id]["position"].append(list(position))
self.trafficLog[id]["velocityNED"].append(velocity)
self.trafficLog[id]["positionNED"].append(positionLoc)
def WriteLog(self, logname=""):
self.ownshipLog['localPlans'] = self.localPlans
self.ownshipLog['localFences'] = self.localFences
if logname == "":
logname = self.callsign + '.json'
import json
if self.verbose > 0:
print("writing log: %s" % logname)
log_data = {"ownship": self.ownshipLog,
"traffic": self.trafficLog,
"waypoints": self.flightplan1,
"geofences": self.fenceList,
"parameters": self.params,
"mergefixes": self.localMergeFixes,
"sim_type": "pyIcarous"}
with open(logname, 'w') as f:
json.dump(log_data, f)
def Run(self):
time_now = time.time()
if not self.fasttime:
if time_now - self.currTime >= 0.05:
self.currTime = time_now
else:
return False
else:
self.currTime += 0.05
#time_cog_in = time.time()
self.RunCognition()
#time_cog_out = time.time()
#time_traff_in = time.time()
self.RunTrafficMonitor()
#time_traff_out = time.time()
#time_geof_in = time.time()
self.RunGeofenceMonitor()
#time_geof_out = time.time()
#time_guid_in = time.time()
self.RunGuidance()
#time_guid_out = time.time()
self.RunMerger()
#time_ship_in = time.time()
self.RunOwnship()
#time_ship_out = time.time()
#print("cog %f" % (time_cog_out - time_cog_in))
#print("traffic %f" % (time_traff_out - time_traff_in))
#print("geofence %f" % (time_geof_out - time_geof_in))
#print("ownship %f" % (time_ship_out - time_ship_in))
return True
def __init__(self,
init_pose=None,
init_velocities=None,
init_angle_velocities=None,
runtime=5.,
target_pos=None,
factor=None):
"""Initialize a Task object.
Params
======
init_pose: initial position of the quadcopter in (x,y,z) dimensions and the Euler angles
init_velocities: initial velocity of the quadcopter in (x,y,z) dimensions
init_angle_velocities: initial radians/second for each of the three Euler angles
runtime: time limit for each episode
target_pos: target/goal (x,y,z) position for the agent
"""
# Simulation
#self.sim = PhysicsSim(init_pose, init_velocities, init_angle_velocities, runtime)
# Define the quadcopters
QUADCOPTER = {
'q1': {
'position': init_pose[:3],
'orientation': [0, 0, 0],
'L': 0.3,
'r': 0.1,
'prop_size': [10, 4.5],
'weight': 1.2
}
}
self.sim = QuadSim(QUADCOPTER)
self.action_repeat = 1
# Stat reporting
self.reward_arr = []
# Store Rotor behaviors
self.rotor_arr = []
# Noise tracking
self.noise_arr = []
# Action space
self.action_low = 4000
self.action_high = 9000 #5500 #9000
self.action_size = 4
# Variable factor for debug
self.factor = factor
self.randomize = False
# Goal
self.target_pos = target_pos if target_pos is not None else np.array(
[0., 0., 10.])
self.state_size = self.action_repeat * (self.get_state()).shape[0] #6
self.init_pose = init_pose
self.runtime = runtime
self.reached = False
self.dist = 0
self.agent_rotor_speeds = [0, 0, 0, 0]
self.pid_rotor_speeds = [0, 0, 0, 0]
# Controller parameters
CONTROLLER_PARAMETERS = {
'Motor_limits': [4000, 9000], #9000
'Tilt_limits': [-10, 10],
'Yaw_Control_Limits': [-900, 900],
'Z_XY_offset': 500,
'Linear_PID': {
'P': [300, 300, 7000],
'I': [0.04, 0.04, 4.5],
'D': [450, 450, 5000]
},
'Linear_To_Angular_Scaler': [1, 1, 0],
'Yaw_Rate_Scaler': 0.18,
'Angular_PID': {
'P': [22000, 22000, 1500],
'I': [0, 0, 1.2],
'D': [12000, 12000, 0]
},
}
self.ctr = controller.Controller_PID_Point2Point(
self.sim.get_state,
self.sim.get_time,
self.sim.set_motor_speeds,
params=CONTROLLER_PARAMETERS,
quad_identifier='q1')
class M_Task():
"""Task (environment) that defines the goal and provides feedback to the agent."""
def __init__(self,
init_pose=None,
init_velocities=None,
init_angle_velocities=None,
runtime=5.,
target_pos=None,
factor=None):
"""Initialize a Task object.
Params
======
init_pose: initial position of the quadcopter in (x,y,z) dimensions and the Euler angles
init_velocities: initial velocity of the quadcopter in (x,y,z) dimensions
init_angle_velocities: initial radians/second for each of the three Euler angles
runtime: time limit for each episode
target_pos: target/goal (x,y,z) position for the agent
"""
# Simulation
#self.sim = PhysicsSim(init_pose, init_velocities, init_angle_velocities, runtime)
# Define the quadcopters
QUADCOPTER = {
'q1': {
'position': init_pose[:3],
'orientation': [0, 0, 0],
'L': 0.3,
'r': 0.1,
'prop_size': [10, 4.5],
'weight': 1.2
}
}
self.sim = QuadSim(QUADCOPTER)
self.action_repeat = 1
# Stat reporting
self.reward_arr = []
# Store Rotor behaviors
self.rotor_arr = []
# Noise tracking
self.noise_arr = []
# Action space
self.action_low = 4000
self.action_high = 9000 #5500 #9000
self.action_size = 4
# Variable factor for debug
self.factor = factor
self.randomize = False
# Goal
self.target_pos = target_pos if target_pos is not None else np.array(
[0., 0., 10.])
self.state_size = self.action_repeat * (self.get_state()).shape[0] #6
self.init_pose = init_pose
self.runtime = runtime
self.reached = False
self.dist = 0
self.agent_rotor_speeds = [0, 0, 0, 0]
self.pid_rotor_speeds = [0, 0, 0, 0]
# Controller parameters
CONTROLLER_PARAMETERS = {
'Motor_limits': [4000, 9000], #9000
'Tilt_limits': [-10, 10],
'Yaw_Control_Limits': [-900, 900],
'Z_XY_offset': 500,
'Linear_PID': {
'P': [300, 300, 7000],
'I': [0.04, 0.04, 4.5],
'D': [450, 450, 5000]
},
'Linear_To_Angular_Scaler': [1, 1, 0],
'Yaw_Rate_Scaler': 0.18,
'Angular_PID': {
'P': [22000, 22000, 1500],
'I': [0, 0, 1.2],
'D': [12000, 12000, 0]
},
}
self.ctr = controller.Controller_PID_Point2Point(
self.sim.get_state,
self.sim.get_time,
self.sim.set_motor_speeds,
params=CONTROLLER_PARAMETERS,
quad_identifier='q1')
def hover(self):
self.ctr.update_target(self.target_pos)
self.ctr.update_yaw_target(0.)
def euler_to_quaternion(self, roll, pitch, yaw):
qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos(
roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2)
qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin(
roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2)
qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin(
roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2)
qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin(
roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2)
return [qx, qy, qz, qw]
def tan_reward(self, x):
return -np.tanh((10 * x) - 3)
def get_reward(self):
reward = 0
position = np.copy(self.sim.get_position('q1'))
velocity = self.sim.get_linear_rate('q1')
euler_angles = self.sim.get_orientation('q1')
# Main reward
self.dist = np.linalg.norm(position - self.target_pos)
difference = abs(
np.subtract(self.pid_rotor_speeds, self.agent_rotor_speeds))
normalized = sum(np.divide(difference, [5000, 5000, 5000, 5000])) / 4
reward = self.tan_reward(normalized)
# Reached target
if (self.dist <= 1):
self.hover()
self.reached = True
return [reward]
def pretty(self, obj, labels=None):
if (labels != None):
for i, l in zip(obj, labels):
print(l + " {:4.3f} ".format(i), end=" ")
else:
for i, n in enumerate(obj):
print(str(i) + ": {:4.3f} ".format(n), end=" ")
print("\n")
# Changing state space
def get_state(self):
dist_err = np.subtract(self.sim.get_position('q1'), self.target_pos)
orientation = self.sim.get_orientation('q1')
angular = self.sim.get_angular_rate('q1')
linear = self.sim.get_linear_rate('q1')
vec = [dist_err, orientation, angular, linear]
out = []
for i, active in enumerate(self.factor):
if (active == 1):
out = np.concatenate((out, vec[i]))
return np.array(out)
def step(self, agent_rotor_speeds, noise=None):
"""Uses action to obtain next state, reward, done."""
reward = 0.
pose_all = []
done = False
self.pid_rotor_speeds = self.ctr.update()
self.agent_rotor_speeds = agent_rotor_speeds
for _ in range(self.action_repeat):
self.sim.set_motor_speeds(
'q1',
(self.pid_rotor_speeds)) # update the sim pose and velocities
bounds = self.sim.update(1 / 50.)
rewards = self.get_reward()
# Run out of time or out of bounds
if (self.sim.time > self.runtime):
if bounds:
print("Out of bounds: ", self.sim.pose[:3])
done = True
reward += sum(rewards)
pose_all.append(self.get_state())
# For plotting
self.rotor_arr.append(agent_rotor_speeds)
self.reward_arr.append(rewards)
self.noise_arr.append(noise)
next_state = np.concatenate(pose_all)
return next_state, reward, done
def reset(self):
"""Reset the sim to start a new episode."""
self.sim.reset()
self.reached = False
# -10 to 10 (x,y)
# 0 to 10 (z)
if (self.randomize): # New start location
rx = rn.randint(-1, 2)
ry = rn.randint(-1, 2)
rz = rn.randint(-1, 2)
new_start = np.add([rx, ry, rz], self.init_pose[:3])
print("New start: ", new_start)
self.sim.set_position('q1', new_start)
else:
self.sim.set_position('q1', self.init_pose[:3])
self.sim.set_orientation('q1', self.init_pose[3:])
self.sim.time = 0.
state = np.concatenate([self.get_state()] * self.action_repeat)
# Refresh inner episodic stats
self.reward_arr = []
self.rotor_arr = []
self.noise_arr = []
return state
def __init__(self,
initialPos,
vehicleSpeed,
targetPosLLA=None,
targetPosNED=None,
simtype="UAS_ROTOR",
fasttime=True):
"""
@initialPos (lat,lon) tuple containing starting
latitude, longitude of simulation
@targetPos (rangeN,rangeE,rangeD) tuple containing
N,E,D range to target position
"""
self.fasttime = fasttime
self.home_pos = [initialPos[0], initialPos[1], initialPos[2]]
self.traffic = []
if simtype == "UAM_VTOL":
self.ownship = VehicleSim(0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
else:
self.ownship = QuadSim()
if targetPosNED is not None:
self.targetPos = np.array(
[targetPosNED[1], targetPosNED[0], targetPosNED[2]])
else:
targetN = distance(self.home_pos[0], self.home_pos[1],
targetPosLLA[0], self.home_pos[1])
targetE = distance(self.home_pos[0], self.home_pos[1],
self.home_pos[0], targetPosLLA[1])
if (targetPosLLA[0] > self.home_pos[0]):
signN = 1
else:
signN = -1
if (targetPosLLA[1] > self.home_pos[1]):
signE = 1
else:
signE = -1
self.targetPos = np.array(
[signE * targetE, signN * targetN, targetPosLLA[2]])
self.simSpeed = vehicleSpeed
self.currentOwnshipPos = (0, 0, initialPos[2])
self.currentOwnshipVel = (0, 0, 0)
self.ownshipPosLog = []
self.ownshipVelLog = []
self.trafficPosLog = []
self.trafficVelLog = []
self.tfMonitor = TrafficMonitor(0)
self.preferredTrack = []
self.preferredSpeed = []
self.preferredAlt = []
self.dist = 200
self.count = 0
self.ptrack = 0
self.pspeed = 0
self.palt = 0
self.pvs = 0
self.conflict = 0
self.cleared = False
self.heading2target = 0
self.trackResolution = True
self.speedResolution = False
self.altResolution = False
self.resObtained = False
self.resType = 0
self.relpos = []
self.relvel = []
self.confcount = 0
class IcarousSim():
def __init__(self,
initialPos,
vehicleSpeed,
targetPosLLA=None,
targetPosNED=None,
simtype="UAS_ROTOR",
fasttime=True):
"""
@initialPos (lat,lon) tuple containing starting
latitude, longitude of simulation
@targetPos (rangeN,rangeE,rangeD) tuple containing
N,E,D range to target position
"""
self.fasttime = fasttime
self.home_pos = [initialPos[0], initialPos[1], initialPos[2]]
self.traffic = []
if simtype == "UAM_VTOL":
self.ownship = VehicleSim(0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
else:
self.ownship = QuadSim()
if targetPosNED is not None:
self.targetPos = np.array(
[targetPosNED[1], targetPosNED[0], targetPosNED[2]])
else:
targetN = distance(self.home_pos[0], self.home_pos[1],
targetPosLLA[0], self.home_pos[1])
targetE = distance(self.home_pos[0], self.home_pos[1],
self.home_pos[0], targetPosLLA[1])
if (targetPosLLA[0] > self.home_pos[0]):
signN = 1
else:
signN = -1
if (targetPosLLA[1] > self.home_pos[1]):
signE = 1
else:
signE = -1
self.targetPos = np.array(
[signE * targetE, signN * targetN, targetPosLLA[2]])
self.simSpeed = vehicleSpeed
self.currentOwnshipPos = (0, 0, initialPos[2])
self.currentOwnshipVel = (0, 0, 0)
self.ownshipPosLog = []
self.ownshipVelLog = []
self.trafficPosLog = []
self.trafficVelLog = []
self.tfMonitor = TrafficMonitor(0)
self.preferredTrack = []
self.preferredSpeed = []
self.preferredAlt = []
self.dist = 200
self.count = 0
self.ptrack = 0
self.pspeed = 0
self.palt = 0
self.pvs = 0
self.conflict = 0
self.cleared = False
self.heading2target = 0
self.trackResolution = True
self.speedResolution = False
self.altResolution = False
self.resObtained = False
self.resType = 0
self.relpos = []
self.relvel = []
self.confcount = 0
def setpos_uncertainty_ownship(self, xx, yy, zz, xy, yz, xz, coeff=0.8):
self.ownship.setpos_uncertainty(xx, yy, zz, xy, yz, xz, coeff)
def setpos_uncertainty_traffic(self, xx, yy, zz, xy, yz, xz, coeff=0.8):
for tf in self.traffic:
tf.setpos_uncertainty(xx, yy, zz, xy, yz, xz, coeff)
def InputTraffic(self, rng, brng, alt, speed, heading, crate):
tx = rng * np.sin(brng * np.pi / 180)
ty = rng * np.cos(brng * np.pi / 180)
tz = alt
tvx = speed * np.sin(heading * np.pi / 180)
tvy = speed * np.cos(heading * np.pi / 180)
tvz = crate
self.traffic.append(VehicleSim(0.05, tx, ty, tz, tvx, tvy, tvz))
self.trafficPosLog.append([])
self.trafficVelLog.append([])
def Cleanup(self):
self.tfMonitor.deleteobj()
def Run(self):
time_prev = 0
while (self.dist > 10):
time_now = time.time()
if not self.fasttime:
if time_now - time_prev >= 0.05:
time_prev = time_now
else:
continue
self.count += 1
if (self.count > 2000) and (self.dist >= 1000):
print("terminating simulation")
break
U = (0, 0, 0)
self.dist = np.linalg.norm(self.targetPos - self.currentOwnshipPos)
if self.conflict == 0:
U = ComputeControl(self.simSpeed, self.currentOwnshipPos,
self.targetPos)
self.resObtained = False
else:
(ve, vn, vd) = (0, 0, 0)
if self.trackResolution:
#print("executing track resolution")
(ve, vn,
vd) = ConvertTrkGsVsToVned(self.ptrack, self.simSpeed, 0)
elif self.speedResolution:
#print("executing speed resolution")
(ve, vn,
vd) = ConvertTrkGsVsToVned(self.heading2target,
self.pspeed, 0)
elif self.altResolution:
#print("executing alt resolution")
(ve, vn,
vd) = ConvertTrkGsVsToVned(self.heading2target,
self.simSpeed, self.pvs)
U = np.array([ve, vn, vd])
self.ownship.input(U[0], U[1], U[2])
self.ownship.step()
opos = self.ownship.getOutputPosition()
ovel = self.ownship.getOutputVelocity()
self.currentOwnshipPos = (opos[0], opos[1],
opos[2] + self.home_pos[2])
self.currentOwnshipVel = (ovel[0], ovel[1], ovel[2])
self.ownshipPosLog.append(self.currentOwnshipPos)
self.ownshipVelLog.append(self.currentOwnshipVel)
home_pos = self.home_pos
targetPos = self.targetPos
(ogx, ogy) = gps_offset(home_pos[0], home_pos[1],
self.currentOwnshipPos[0],
self.currentOwnshipPos[1])
(wgx, wgy) = gps_offset(home_pos[0], home_pos[1], targetPos[0],
targetPos[1])
ownship_pos_gx = [ogx, ogy, self.currentOwnshipPos[2]]
ownship_vel = [
self.currentOwnshipVel[0], self.currentOwnshipVel[1],
self.currentOwnshipVel[2]
]
ovelTrkGsVs = ConvertVnedToTrkGsVs(*ownship_vel)
self.heading2target = ComputeHeading(ownship_pos_gx, targetPos)
traffic_pos_gx = []
traffic_vel = []
for i in range(len(self.traffic)):
traffic = self.traffic[i]
oldvel = traffic.getOutputVelocity()
traffic.input(oldvel[0], oldvel[1], oldvel[2])
traffic.step()
newpos = traffic.getOutputPosition()
newvel = traffic.getOutputVelocity()
self.trafficPosLog[i].append(newpos)
self.trafficVelLog[i].append(newvel)
(tgx, tgy) = gps_offset(home_pos[0], home_pos[1],
traffic.pos[0], traffic.pos[1])
traffic_pos_gx = [tgx, tgy, traffic.pos[2]]
traffic_vel = [traffic.vel[0], traffic.vel[1], traffic.vel[2]]
self.tfMonitor.input_traffic(i, traffic_pos_gx, traffic_vel,
self.count * 0.05)
self.tfMonitor.monitor_traffic(ownship_pos_gx, ovelTrkGsVs,
self.count * 0.05)
(conflict1, self.ptrack) = self.tfMonitor.GetTrackBands()
(conflict2, self.pspeed) = self.tfMonitor.GetGSBands()
(conflict3, pdown, pup, self.palt) = self.tfMonitor.GetAltBands()
self.conflict = (conflict1 == 1) or (conflict2 == 1) or (conflict3
== 1)
if (self.conflict is True) and not self.resObtained:
self.confcount = 1
print("conflict detected")
self.resObtained = True
# Place holder for to use functions to obtain resolutions
wpFeasibility = False
if self.resObtained:
if self.trackResolution:
if not math.isfinite(self.ptrack) or self.ptrack == -1:
if len(self.preferredTrack) > 0:
self.ptrack = self.preferredTrack[-1]
else:
self.ptrack = -1
currentHeading = ovelTrkGsVs[0]
safe2Turn = self.tfMonitor.check_safe_to_turn(
ownship_pos_gx, ovelTrkGsVs, currentHeading,
self.heading2target)
newOvelTrkGsVs = ovelTrkGsVs
newOvelTrkGsVs = (self.heading2target, ovelTrkGsVs[1],
ovelTrkGsVs[2])
wpFeasibility = self.tfMonitor.monitor_wp_feasibility(
ownship_pos_gx, ovelTrkGsVs, [wgx, wgy, targetPos[2]])
if not wpFeasibility:
self.conflict = 1
if self.ptrack == -1:
self.ptrack = self.preferredTrack[-1]
self.preferredTrack.append(self.ptrack)
elif self.speedResolution:
if not math.isfinite(self.pspeed):
if len(self.preferredSpeed) > 0:
self.pspeed = self.preferredSpeed[-1]
else:
self.pspeed = -100
self.vehicleSpeed.append(ovelTrkGsVs[1])
ovelTrkGsVs = (ovelTrkGsVs[0], self.simSpeed,
ovelTrkGsVs[2])
wpFeasibility = self.tfMonitor.monitor_wp_feasibility(
ownship_pos_gx, ovelTrkGsVs, [wgx, wgy, targetPos[2]])
if not wpFeasibility:
self.conflict = 1
if len(self.preferredSpeed) > 0:
self.pSpeed = self.preferredSpeed[-1]
else:
self.pSpeed = -100
self.preferredSpeed.append(self.pspeed)
if self.pspeed == -100:
self.pspeed = self.simSpeed
elif self.altResolution:
self.palt = pup + 2
if not math.isfinite(self.palt):
if len(self.preferredAlt) > 0:
self.palt = self.preferredAlt[-1]
else:
self.palt = -1000
wpFeasibility = self.tfMonitor.monitor_wp_feasibility(
ownship_pos_gx, ovelTrkGsVs, [wgx, wgy, targetPos[2]])
if not wpFeasibility:
self.conflict = 1
if self.conflict:
diffAlt = self.palt - ownship_pos_gx[2]
self.pvs = 0.1 * diffAlt
self.preferredAlt.append(self.palt)
if self.palt == -1000:
self.palt = 5