def RunOwnship(self): trkgsvs_command = ConvertVnedToTrkGsVs(*self.controlInput) self.ownship.InputCommand(*trkgsvs_command) self.ownship.Run(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.RecordOwnship()
def RunOwnship(self): self.controlInput = self.core.GetOutput() self.ownship.InputCommand(*self.controlInput) self.ownship.Run(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.RecordOwnship() sigmaPos = [ self.ownship.sigma_pos[0][0], self.ownship.sigma_pos[1][1], self.ownship.sigma_pos[2][2], self.ownship.sigma_pos[0][1], self.ownship.sigma_pos[0][2], self.ownship.sigma_pos[1][2] ] sigmaVel = [ self.ownship.sigma_vel[0][0], self.ownship.sigma_vel[1][1], self.ownship.sigma_vel[2][2], self.ownship.sigma_vel[0][1], self.ownship.sigma_vel[0][2], self.ownship.sigma_vel[1][2] ] self.core.InputOwnshipState(self.currTime, self.position, self.trkgsvs, sigmaPos, sigmaVel)
def __init__(self, idx, home_gps, x=0.0, y=0.0, z=0.0, vx=0.0, vy=0.0, vz=0.0, dt=0.05): """ Initialize a UAM VTOL vehicle simulation :param x: initial x position (meters east from home_gps) :param y: initial y position (meters north from home_gps) :param z: initial altitude (meters) :param vx: initial east velocity (m/s) :param vy: initial north velocity (m/s) :param vz: initial upward velocity (m/s) Other arguments are defined in VehicleSimInterface """ super().__init__(idx, home_gps, dt) g = 9.8 self.U = np.array([0.0, 0.0, 0.0]) self.pos0 = np.array([x, y, z]) self.vel0 = np.array([vx, vy, vz]) self.pos = np.array([x, y, z]) self.vel = np.array([vx, vy, vz]) self.turnRate = 20 self.accel = 0.5 * g self.daccel = -0.5 * g self.vaccel = 0.15 * g self.trk, self.gs, self.vs = ConvertVnedToTrkGsVs(vy, vx, vz)
def InputTraffic(self,idx,position,velocity): if idx is not self.vehicleID and 0 not in position: trkgsvs = ConvertVnedToTrkGsVs(velocity[0],velocity[1],velocity[2]) self.tfMonitor.input_traffic(idx,position,trkgsvs,self.currTime) self.Trajectory.InputTrafficData(idx,position,trkgsvs,self.currTime) localPos = self.ConvertToLocalCoordinates(position) self.RecordTraffic(idx, position, velocity, localPos)
def InputTraffic(self, callsign, position, velocity): if callsign != self.callsign and np.abs(np.sum(position)) > 0: trkgsvs = ConvertVnedToTrkGsVs(velocity[0], velocity[1], velocity[2]) self.core.InputIntruderState(self.currTime, callsign, position, trkgsvs) localPos = self.ConvertToLocalCoordinates(position) self.RecordTraffic(callsign, position, velocity, localPos)
def InputTraffic(self, callsign, position, velocity): if callsign != self.callsign and 0 not in position: trkgsvs = ConvertVnedToTrkGsVs(velocity[0], velocity[1], velocity[2]) self.tfMonitor.input_traffic(callsign, position, trkgsvs, self.currTime) self.Trajectory.InputTrafficData(callsign, position, trkgsvs, self.currTime) localPos = self.ConvertToLocalCoordinates(position) self.RecordTraffic(callsign, position, velocity, localPos)
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 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].latitude,self.flightplan1[nextWP1].longitude) 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.RunCognition(self.currTime) if self.fphase == -2: 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 eta = False if self.activePlan == "Plan0": self.flightplan2 = [] self.nextWP1 = nextWP eta = self.etaFP1 else: self.nextWP2 = nextWP eta = self.etaFP2 self.Guidance.SetGuidanceMode(self.guidanceMode,self.activePlan,nextWP,eta) 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)] waypoints = ConstructWaypointsFromList(fp) self.Guidance.InputFlightplanData("P2P",waypoints) self.activePlan = "P2P" self.nextWP2 = 1 self.Guidance.SetGuidanceMode(self.guidanceMode,self.activePlan,1,False) 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.ALT_CHANGE: altChange = cmd.commandU.altChange.altitude planID= cmd.commandU.altChange.name.decode('utf-8') nextWP = self.nextWP1 if planID == "Plan0" else self.nextWP2 self.Guidance.ChangeWaypointAlt(planID,nextWP,altChange,cmd.commandU.altChange.hold) 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 startPos = cmd.commandU.fpRequest.fromPosition stopPos = cmd.commandU.fpRequest.toPosition startVel = cmd.commandU.fpRequest.startVelocity endVel = cmd.commandU.fpRequest.endVelocity planID = cmd.commandU.fpRequest.name.decode('utf-8') # Find the new path numWP = self.Trajectory.FindPath( planID, startPos, stopPos, startVel, endVel) if numWP > 0: if self.verbose > 0: print("%s : At %f s, Computed flightplan %s with %d waypoints" % (self.callsign,self.currTime,planID,numWP)) # offset the new path with current time (because new paths start with t=0) self.Trajectory.SetPlanOffset(planID,0,self.currTime) # Combine new plan with rest of old plan self.Trajectory.CombinePlan(planID,"Plan0",-1) self.flightplan2 = self.Trajectory.GetFlightPlan(planID) self.Cog.InputFlightplanData(planID,self.flightplan2) self.Guidance.InputFlightplanData(planID,self.flightplan2) # Set guidance mode with new flightplan and wp 0 to offset plan times self.Guidance.SetGuidanceMode(GuidanceMode.FLIGHTPLAN,planID,0,False) self.plans.append(self.flightplan2) self.localPlans.append(self.GetLocalFlightPlan(self.flightplan2)) else: if self.verbose > 0: print(self.callsign,"Error finding path")
def SetInitialConditions(self, x=0, y=0, z=0, vx=0, vy=0, vz=0): self.pos0 = np.array([x, y, z]) self.vel0 = np.array([vx, vy, vz]) self.trk, self.gs, self.vs = ConvertVnedToTrkGsVs(vy, vx, vz)
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