Remote Run-Time Failure Detection and Recovery Control For Quadcopters¶
We propose an adaptive run-time failure recovery control system for quadcopter drones, based on remote real-time processing of measurement data streams. The control system consists of three distinct parts: (1) A set of computationally simple PID controllers locally onboard the drone, (2) a set of computationally more demanding remotely hosted algorithms for real-time drone state detection, and (3) a digital twin co-execution software platform --- the ModelConductor-eXtended --- for two-way signal data exchange between the former two. The local on-board control system is responsible for maneuvering the drone in all conditions: path tracking under normal operation and safe landing in a failure state. The remote control system, on the other hand, is responsible for detecting the state of the drone and communicating the corresponding control commands and controller parameters to the drone in real time. Besides a normal state of operation, two distinct failure states are considered, namely a total failure of one of the motors and linear speed degradation in one of the motors, potentially representing a collision in practice. The proposed control system concept is demonstrated via simulations in which a drone is represented by the widely studied Quad-Sim six degrees-of-freedom Simulink model. The simulations also facilitate rigorous analysis of acceptable communication delays in failure mode recovery. The potential practical applications for the presented approach are in drone operation in complex environments such as factories (indoor) or forests (outdoor).
This notebook performs two main tasks: 1) Load the FMI-wrapped Quad-Sim simulation model (adapted so that the motor RPMs can be measured and PID controllers can be manipulated as well as the motor functionalities), and 2) Establish the communication channel using ModelConductor-eXtended.
Preliminaries¶
Let us examine what we have, i.e., the FMU model and input data.
In [1]:
from fmpy import dump
import pandas as pd
import numpy as np
import json, logging, time, os, random
from datetime import datetime as dt
from modelconductor.utils import Measurement
import traceback
from tqdm import tqdm
from matplotlib import pyplot as plt
step_size = 0.01
fmu_path = "physical_device_simulation_v7\\PC_Quadcopter_Simulation_parametric.fmu"
dump(fmu_path)
Model Info FMI Version 2.0 FMI Type Co-Simulation Model Name PC_Quadcopter_Simulation_parametric Description Platforms darwin64, linux64, win64 Continuous States 0 Event Indicators 0 Variables 41 Generation Tool Simulink (R2020b) Generation Date 2021-06-11T15:06:39Z Default Experiment Stop Time 40 Step Size 0.001 Variables (input, output) Name Causality Start Value Unit Description phi_kp input 0 Input Port: phi_kp phi_ki input 0 Input Port: phi_ki phi_kd input 0 Input Port: phi_kd theta_kp input 0 Input Port: theta_kp theta_ki input 0 Input Port: theta_ki theta_kd input 0 Input Port: theta_kd psi_kp input 0 Input Port: psi_kp psi_ki input 0 Input Port: psi_ki psi_kd input 0 Input Port: psi_kd alt_kp input 0 Input Port: alt_kp alt_ki input 0 Input Port: alt_ki alt_kd input 0 Input Port: alt_kd path_x input 0 Input Port: path_x path_y input 0 Input Port: path_y path_z input 0 Input Port: path_z path_psi input 0 Input Port: path_psi rpm1_coef input 0 Input Port: rpm1_coef rpm2_coef input 0 Input Port: rpm2_coef rpm3_coef input 0 Input Port: rpm3_coef rpm4_coef input 0 Input Port: rpm4_coef pc_theta_kp input 0 Input Port: pc_theta_kp pc_theta_kd input 0 Input Port: pc_theta_kd pc_phi_kp input 0 Input Port: pc_phi_kp pc_phi_kd input 0 Input Port: pc_phi_kd P output Output Port: P Q output Output Port: Q R output Output Port: R X output Output Port: X Y output Output Port: Y Z output Output Port: Z Phi output Output Port: Phi The output Output Port: The Psi output Output Port: Psi U output Output Port: U V output Output Port: V W output Output Port: W RPM1 output Output Port: RPM1 RPM2 output Output Port: RPM2 RPM3 output Output Port: RPM3 RPM4 output Output Port: RPM4 simulation_time output Output Port: simulation_time
Description of inputs and outputs¶
This simulation model is used to mimic the behaviour of a quadcopter drone. In order to do so, RPM values (RPM1 to RPM4) and a few other measurements (velocities, attitude measurement and simulation time) are being sent out to the remote control system.
On the other hand, the gain parameters for 5 different PID controllers are being fed into the simulation model resulting in an adaptive control mechanism over different situations. Additionally, to land the drone safely in the case of failure, its reference path needs to be controlled by the remote controller. Thus the next target point at each time step is given to the drone's simulation (path_x, path_y, etc.). Four RPM coefficients are also considered as input to introduce the failure in the simulation (and also turning the diagonally opposing motor off in case of failure).
Generating random path and failure RPM coefficient¶
The remote controller system reads the measured RPM values and according to a change point detection algorithm followed by a logistic regression classifies the behaviour of the drone as one of failure or normal status. In order to identify the parameters of this failure detection model, we needed to generate training data with random paths and failures occurred in random time instants. This random training data generation is done by the following two methods.
In [ ]:
from modelconductor.modelhandler import FMUModelHandler
def generate_random_path(path_duration=40, n_points=5):
delta = [
2*(np.random.rand(n_points)-0.5),
2*(np.random.rand(n_points)-0.5),
2*(np.random.rand(n_points)-0.5),
np.pi*np.random.rand(n_points)
]
x0 = 0
y0 = 0
z0 = 10
t0 = 0
psi_0 = 0
t = np.random.permutation(np.arange(1, path_duration, 3))[:n_points]
t = np.insert(t, 0, t0)
t = np.sort(t)
data = [[x0], [y0], [z0], [psi_0]]
for i in range(n_points):
for j in range(len(data)):
data[j].append(data[j][-1])
k = np.random.randint(1, 4)
for j in range(k):
m = np.random.randint(0, 4)
if m == 3:
data[m][-1] = delta[m][i]
else:
data[m][-1] += delta[m][i]
return {"x":data[0], "y":data[1], "z":data[2], "psi":data[3], "t":t}
def generate_random_failure_rpm_coefs():
rpm_coefs = {
"rpm1_coef": [1, 0.9*random.random()],
"rpm2_coef": [1, 1],
"rpm3_coef": [1, 1],
"rpm4_coef": [1, 1],
"t": [0, 20 + 10*random.random()]
}
return rpm_coefs
Custom ModelHandler class¶
In the context of ModelConductor-eXtended (MCX) framework, the ModelHandler class is responsible of executing one step of the simulation model with providing the necessary inputs. For this adaptive remote control drone, we have extended the built-in FMUModelHandler class and added the custom behaviour to it. The custom behaviours include: 1) use the PID controller parameters respective to the detected status (or action mode: normal/failure), 2) turn off the diagonally opposing motor in case of failure action mode, 3) land the drone safely in case of failure by over-writting its reference path to reduce its altitude smoothly. You can see the statements for handling these custom behaviours in the step method of this class.
In [2]:
def get_element_or_last(ls, idx):
return ls[min(idx, len(ls)-1)]
class CustomDroneFMUModelHandler(FMUModelHandler):
def __init__(self, fmu_path, step_size, stop_time, timestamp_key,
start_time=None, target_keys=None, input_keys=None,
control_keys=None, controller_parameters=None, path=None, rpm_coefs=None):
super().__init__(fmu_path, step_size, stop_time, timestamp_key,
start_time, target_keys, input_keys, control_keys)
self.controller_parameters = controller_parameters
self.path = path
self.rpm_coefs = rpm_coefs
self.last_path_input = None
self.failure_time = None
self.failure_mode = None
self.landed = False
def get_path_point(self, time):
path_index = 0
while(time >= self.path['t'][path_index]):
path_input = {
"path_x": get_element_or_last(self.path['x'],path_index),
"path_y" : get_element_or_last(self.path['y'],path_index),
"path_z" : get_element_or_last(self.path['z'],path_index),
"path_psi" : get_element_or_last(self.path['psi'],path_index)
}
path_index += 1
if path_index == len(self.path["t"]):
break
return path_input
def get_rpm_coefs_point(self, time):
idx = 0
while(time >= self.rpm_coefs['t'][idx]):
rpm_coefs = {
"rpm1_coef": get_element_or_last(self.rpm_coefs['rpm1_coef'],idx),
"rpm2_coef" : get_element_or_last(self.rpm_coefs['rpm2_coef'],idx),
"rpm3_coef" : get_element_or_last(self.rpm_coefs['rpm3_coef'],idx),
"rpm4_coef" : get_element_or_last(self.rpm_coefs['rpm4_coef'],idx)
}
idx += 1
if idx == len(self.rpm_coefs["t"]):
break
return rpm_coefs
def step(self, X, step_size=None):
action_mode = X['action_mode']
if self.failure_mode is not None:
action_mode = self.failure_mode
params = self.controller_parameters["normal"]
time = X["time"]
if action_mode == "failure_total":
if self.failure_mode is None: # first time that action mode is failure
self.failure_time = time
self.failure_mode = action_mode
path_input = dict(self.last_path_input)
if self.failure_time is not None:
path_input["path_z"] = path_input["path_z"] - (path_input["path_z"]/landing_duration) * (time - self.failure_time)
path_input["path_z"] = max(0, path_input["path_z"])
rpm_coefs = self.get_rpm_coefs_point(time)
rpm_coefs["rpm1_coef"] = 0
rpm_coefs["rpm3_coef"] = 0
if self.landed:
rpm_coefs["rpm2_coef"] = 0
rpm_coefs["rpm4_coef"] = 0
elif action_mode == "failure_partial":
# keep the drone where it was
if self.failure_mode is None: # first time that action mode is failure
self.failure_time = time
self.failure_mode = action_mode
path_input = dict(self.last_path_input)
rpm_coefs = self.get_rpm_coefs_point(time)
elif action_mode == "normal":
path_input = self.get_path_point(time)
rpm_coefs = self.get_rpm_coefs_point(time)
self.last_path_input = path_input
print(f"------ \n",
f"time: {time} \n",
f"action_mode: {action_mode}\n",
"last_path_input", self.last_path_input, "\n",
"current_path_input", path_input, "\n",
f"self.failure_time: {self.failure_time}\n",
f"self.failure_mode: {self.failure_mode}\n ----------")
X_p = {**X, **params, **path_input, **rpm_coefs}
res = super().step(X_p, step_size)
if res["Z"] < 0.001:
self.landed = True
return res
Within ModelConductor, all data that is to be exchanged from one data source / data consumer to another, is asserted to exists in one of two possible formats:
i) A Measurement object that denotes a timestamped data structure that is received from a physical asset, best understood as a snapshot of the asset’s state at a given time or
ii) a ModelResponse object that similarly denotes a timestamped data structure that is received from a digital asset, a snapshot of the model’s virtual state at a given time
A variable (a key) may, but is not required to belong to one or more of the following categories:
1) Input keys: The subset of keys in Measurement object corresponding to the keys that are expected as inputs by the relevant ModelHandler
2) Target keys: The keys that are expected to be output from a ModelHandler instance onto a ModelResponse object
3) Control keys: A subset of keys in Measurement that is not intended to be used as an input to a ModelHandler, but rather as a validation variable against the ModelHandler output
4) Timestamp key: A key denoting the instant when the relevant Measurement or ModelResponce instance was created.
In [3]:
target_keys = ["P", "Q", "R", "Phi", "The", "Psi",
"U", "V", "W", "X", "Y", "Z", "RPM1", "RPM2", "RPM3", "RPM4", "simulation_time"]
input_keys = ["action_mode"] # normal or failure
control_keys = ["time" ,
"TIMING_fd_model_actoin_mode_publish_timestamp",
"TIMING_fmu_model_recieve_action_mode_timestamp",
"TIMING_fmu_model_response_rpms_timestamp",
# "TIMING_fd_model_recieve_rpms_timestamp",
"phi_kp", "phi_ki", "phi_kd",
"theta_kp", "theta_ki", "theta_kd",
"psi_kp", "psi_ki", "psi_kd",
"alt_kp", "alt_ki", "alt_kd",
'pc_phi_kp', 'pc_phi_kd',
'pc_theta_kp', 'pc_theta_kd',
"path_x", "path_y", "path_z", "path_psi",
"rpm1_coef", "rpm2_coef", "rpm3_coef", "rpm4_coef"]
timestamp_key = "time"
partial_target_keys = [ "time", "simulation_time", "RPM1", "RPM2", "RPM3", "RPM4",
"TIMING_fd_model_actoin_mode_publish_timestamp",
"TIMING_fmu_model_recieve_action_mode_timestamp",
"TIMING_fmu_model_response_rpms_timestamp",]
controller_parameters = {
'normal': {
'phi_kp': 2, 'phi_ki': 1.1, 'phi_kd': 1.2,
'theta_kp': 2, 'theta_ki': 1.1, 'theta_kd': 1.2,
'psi_kp': 4, 'psi_ki': 0.5, 'psi_kd': 3.5,
'alt_kp': 2, 'alt_ki': 1.1, 'alt_kd': 3.3,
'pc_phi_kp': 0.32, 'pc_phi_kd': 0.1,
'pc_theta_kp': 0.32, 'pc_theta_kd': 0.1,
},
'failure_total': { # total failure -> land
'phi_kp': 4.5, 'phi_ki': 2.7, 'phi_kd': 0.53,
'theta_kp': 7.8, 'theta_ki': 2.7, 'theta_kd': 0.53,
'psi_kp': 0, 'psi_ki': 0, 'psi_kd': 0,
'alt_kp': 700, 'alt_ki': 10, 'alt_kd': 200,
'pc_phi_kp': 0, 'pc_phi_kd': 0,
'pc_theta_kp': 0, 'pc_theta_kd': 0,
},
'failure_partial': { # partial failure -> continue
'phi_kp': 10, 'phi_ki': 3, 'phi_kd': 12,
'theta_kp': 10, 'theta_ki': 3, 'theta_kd': 12,
'psi_kp': 15, 'psi_ki': 2, 'psi_kd': 5,
'alt_kp': 15, 'alt_ki': 3, 'alt_kd': 5,
'pc_phi_kp': 0.32, 'pc_phi_kd': 0.1,
'pc_theta_kp': 0.32, 'pc_theta_kd': 0.1,
}
}
paper_path = {
"x" : [ 0, 2, 2, 4, 4, 5, 5, 7, 7, 9, 9, 9, 9, 9, 9, 9],
"y" : [ 0, 0, 2, 2, 1, 1, -1, -1, 2, 2, 5, 4, 4, 4, 4, 4],
"z" : [ 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 2, 1.5, 1.25, 1, 0],
"t" : [ 0, 2, 4, 6, 7, 8, 10, 12,15,17,20,21, 23, 25,27, 30],
"psi": [0]
}
rpm_coefs = {
"rpm1_coef": [1, 0.75],
"rpm2_coef": [1, 1],
"rpm3_coef": [1, 1],
"rpm4_coef": [1, 1],
"t": [0, 15]
}
landing_duration = 4
Now the ModelHandler object can be instantiated:
In [4]:
model=CustomDroneFMUModelHandler(
fmu_path=fmu_path,
start_time=0,
stop_time=25,
step_size=step_size,
target_keys=target_keys,
input_keys=input_keys,
control_keys=control_keys,
timestamp_key=timestamp_key,
controller_parameters=controller_parameters,
path=paper_path,
rpm_coefs=rpm_coefs)
The experiment¶
The experiment object will use the model inside its subscriber. The broker will start for listening to new measurements from publishers and the subscriber will be listening for new measurements which comes from the broker.
In [5]:
import asyncio
import nest_asyncio
import json, logging, time
from hbmqtt.client import MQTTClient, ConnectException
from hbmqtt.errors import MQTTException
from hbmqtt.mqtt.constants import QOS_0, QOS_1, QOS_2
from modelconductor.experiment_mqtt import MqttExperiment
from modelconductor.utils import Measurement
logger = logging.getLogger(__name__)
formatter = "[%(asctime)s] :: %(levelname)s - %(message)s"
logging.basicConfig(level=logging.INFO, format=formatter)
nest_asyncio.apply()
class CustomDroneMqttExperiment(MqttExperiment):
def __init__(self, partial_target_keys = [], **kwargs):
super().__init__(**kwargs)
self.partial_target_keys = partial_target_keys
@asyncio.coroutine
def do_sub(self, client, url):
try:
print("hie")
loop = asyncio.get_event_loop()
yield from client.connect(uri=url)
qos = QOS_1
filters = [("drone/input_actions", qos)]
yield from client.subscribe(filters)
self.model.spawn()
count = 0
while True:
try:
message = yield from client.deliver_message()
count += 1
item = message.publish_packet.data
data = json.loads(item.decode('utf-8'))
data['TIMING_fmu_model_recieve_action_mode_timestamp'] = time.time()
data = Measurement(data)
print(data)
data['TIMING_fmu_model_response_rpms_timestamp'] = 0
res = self.model.step(data)
res = {**res, **data}
res['TIMING_fmu_model_response_rpms_timestamp'] = time.time()
partial_res = {k: res[k] for k in self.partial_target_keys}
print("result: ")
print(res)
message = json.dumps(partial_res).encode(encoding='utf-8')
loop.run_until_complete(client.publish("drone/model_responses", message, qos))
self.results.append(res)
self.log_row(res, self.model)
except MQTTException:
logger.debug("Error reading packet")
yield from client.disconnect()
except KeyboardInterrupt:
yield from client.disconnect()
except ConnectException as ce:
logger.fatal("connection to '%s' failed: %r" % (url, ce))
except asyncio.CancelledError as cae:
logger.fatal("Publish canceled due to prvious error: %r" % cae)
In [6]:
ex = CustomDroneMqttExperiment(partial_target_keys=partial_target_keys)
ex.model = model
headers = ["timestamp"]
if input_keys:
headers += input_keys
if target_keys:
headers += target_keys
if control_keys:
headers += control_keys
ex.logger = ex.initiate_logging(headers=headers,
path=ex.log_path)
Run¶
Run the receiving loop. When it succeeded starting run the client on another process with this command:
python client_mqtt.py
In [7]:
try:
ex.run()
except Exception as e:
model.destroy()
raise e
except KeyboardInterrupt as e:
print(e)
model.destroy()
raise e
running execute_model_loop! <class '__main__.CustomDroneFMUModelHandler'> hie
[2021-06-24 01:27:33,810] :: INFO - Finished processing state new exit callbacks. [2021-06-24 01:27:33,810] :: INFO - Finished processing state connected enter callbacks.
[OK] %s
[OK] %s
{'action_mode': 'normal', 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487262.9629629, 'time': 0, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487262.964958}
------
time: 0
action_mode: normal
last_path_input {'path_x': 0, 'path_y': 0, 'path_z': 3, 'path_psi': 0}
current_path_input {'path_x': 0, 'path_y': 0, 'path_z': 3, 'path_psi': 0}
self.failure_time: None
self.failure_mode: None
----------
result:
{'phi_kp': 2, 'phi_ki': 1.1, 'phi_kd': 1.2, 'theta_kp': 2, 'theta_ki': 1.1, 'theta_kd': 1.2, 'psi_kp': 4, 'psi_ki': 0.5, 'psi_kd': 3.5, 'alt_kp': 2, 'alt_ki': 1.1, 'alt_kd': 3.3, 'path_x': 0, 'path_y': 0, 'path_z': 3, 'path_psi': 0, 'rpm1_coef': 1, 'rpm2_coef': 1, 'rpm3_coef': 1, 'rpm4_coef': 1, 'pc_theta_kp': 0.32, 'pc_theta_kd': 0.1, 'pc_phi_kp': 0.32, 'pc_phi_kd': 0.1, 'P': 0.0, 'Q': 0.0, 'R': 0.0, 'X': 0.0, 'Y': 0.0, 'Z': 9.99987908663846, 'Phi': 0.0, 'The': 0.0, 'Psi': 0.0, 'U': 0.0, 'V': 0.0, 'W': -0.01675402482221448, 'RPM1': 3887.313979129054, 'RPM2': 3887.313979129054, 'RPM3': 3887.313979129054, 'RPM4': 3887.313979129054, 'simulation_time': 0.01, 'time': 0, 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487262.9629629, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487262.964958, 'TIMING_fmu_model_response_rpms_timestamp': 1624487263.0189316, 'action_mode': 'normal'}
{'action_mode': 'normal', 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487263.0229325, 'time': 0.01, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487263.0249321}
------
time: 0.01
action_mode: normal
last_path_input {'path_x': 0, 'path_y': 0, 'path_z': 3, 'path_psi': 0}
current_path_input {'path_x': 0, 'path_y': 0, 'path_z': 3, 'path_psi': 0}
self.failure_time: None
self.failure_mode: None
----------
result:
{'phi_kp': 2, 'phi_ki': 1.1, 'phi_kd': 1.2, 'theta_kp': 2, 'theta_ki': 1.1, 'theta_kd': 1.2, 'psi_kp': 4, 'psi_ki': 0.5, 'psi_kd': 3.5, 'alt_kp': 2, 'alt_ki': 1.1, 'alt_kd': 3.3, 'path_x': 0, 'path_y': 0, 'path_z': 3, 'path_psi': 0, 'rpm1_coef': 1, 'rpm2_coef': 1, 'rpm3_coef': 1, 'rpm4_coef': 1, 'pc_theta_kp': 0.32, 'pc_theta_kd': 0.1, 'pc_phi_kp': 0.32, 'pc_phi_kd': 0.1, 'P': 0.0, 'Q': 0.0, 'R': 0.0, 'X': 0.0, 'Y': 0.0, 'Z': 9.999651535981753, 'Phi': 0.0, 'The': 0.0, 'Psi': 0.0, 'U': 0.0, 'V': 0.0, 'W': -0.029589246876252487, 'RPM1': 3775.0141113717136, 'RPM2': 3775.0141113717136, 'RPM3': 3775.0141113717136, 'RPM4': 3775.0141113717136, 'simulation_time': 0.02, 'time': 0.01, 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487263.0229325, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487263.0249321, 'TIMING_fmu_model_response_rpms_timestamp': 1624487263.0509343, 'action_mode': 'normal'}
{'action_mode': 'normal', 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487263.0549316, 'time': 0.02, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487263.0559325}
------
time: 0.02
action_mode: normal
last_path_input {'path_x': 0, 'path_y': 0, 'path_z': 3, 'path_psi': 0}
current_path_input {'path_x': 0, 'path_y': 0, 'path_z': 3, 'path_psi': 0}
self.failure_time: None
self.failure_mode: None
----------
result:
{'phi_kp': 2, 'phi_ki': 1.1, 'phi_kd': 1.2, 'theta_kp': 2, 'theta_ki': 1.1, 'theta_kd': 1.2, 'psi_kp': 4, 'psi_ki': 0.5, 'psi_kd': 3.5, 'alt_kp': 2, 'alt_ki': 1.1, 'alt_kd': 3.3, 'path_x': 0, 'path_y': 0, 'path_z': 3, 'path_psi': 0, 'rpm1_coef': 1, 'rpm2_coef': 1, 'rpm3_coef': 1, 'rpm4_coef': 1, 'pc_theta_kp': 0.32, 'pc_theta_kd': 0.1, 'pc_phi_kp': 0.32, 'pc_phi_kd': 0.1, 'P': 0.0, 'Q': 0.0, 'R': 0.0, 'X': 0.0, 'Y': 0.0, 'Z': 9.999271886907376, 'Phi': 0.0, 'The': 0.0, 'Psi': 0.0, 'U': 0.0, 'V': 0.0, 'W': -0.04705279252015821, 'RPM1': 3676.2951468103934, 'RPM2': 3676.2951468103934, 'RPM3': 3676.2951468103934, 'RPM4': 3676.2951468103934, 'simulation_time': 0.03, 'time': 0.02, 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487263.0549316, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487263.0559325, 'TIMING_fmu_model_response_rpms_timestamp': 1624487263.0829346, 'action_mode': 'normal'}
{'action_mode': 'normal', 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487263.086935, 'time': 0.03, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487263.0889373}
------
time: 0.03
action_mode: normal
last_path_input {'path_x': 0, 'path_y': 0, 'path_z': 3, 'path_psi': 0}
current_path_input {'path_x': 0, 'path_y': 0, 'path_z': 3, 'path_psi': 0}
self.failure_time: None
self.failure_mode: None
----------
result:
{'phi_kp': 2, 'phi_ki': 1.1, 'phi_kd': 1.2, 'theta_kp': 2, 'theta_ki': 1.1, 'theta_kd': 1.2, 'psi_kp': 4, 'psi_ki': 0.5, 'psi_kd': 3.5, 'alt_kp': 2, 'alt_ki': 1.1, 'alt_kd': 3.3, 'path_x': 0, 'path_y': 0, 'path_z': 3, 'path_psi': 0, 'rpm1_coef': 1, 'rpm2_coef': 1, 'rpm3_coef': 1, 'rpm4_coef': 1, 'pc_theta_kp': 0.32, 'pc_theta_kd': 0.1, 'pc_phi_kp': 0.32, 'pc_phi_kd': 0.1, 'P': 0.0, 'Q': 0.0, 'R': 0.0, 'X': 0.0, 'Y': 0.0, 'Z': 9.998697292814773, 'Phi': 0.0, 'The': 0.0, 'Psi': 0.0, 'U': 0.0, 'V': 0.0, 'W': -0.06847575600949395, 'RPM1': 3589.627296343102, 'RPM2': 3589.627296343102, 'RPM3': 3589.627296343102, 'RPM4': 3589.627296343102, 'simulation_time': 0.04, 'time': 0.03, 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487263.086935, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487263.0889373, 'TIMING_fmu_model_response_rpms_timestamp': 1624487263.1129324, 'action_mode': 'normal'}
{'action_mode': 'normal', 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487263.1179338, 'time': 0.04, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487263.1189337}
------
time: 0.04
action_mode: normal
last_path_input {'path_x': 0, 'path_y': 0, 'path_z': 3, 'path_psi': 0}
current_path_input {'path_x': 0, 'path_y': 0, 'path_z': 3, 'path_psi': 0}
self.failure_time: None
self.failure_mode: None
----------
result:
{'phi_kp': 2, 'phi_ki': 1.1, 'phi_kd': 1.2, 'theta_kp': 2, 'theta_ki': 1.1, 'theta_kd': 1.2, 'psi_kp': 4, 'psi_ki': 0.5, 'psi_kd': 3.5, 'alt_kp': 2, 'alt_ki': 1.1, 'alt_kd': 3.3, 'path_x': 0, 'path_y': 0, 'path_z': 3, 'path_psi': 0, 'rpm1_coef': 1, 'rpm2_coef': 1, 'rpm3_coef': 1, 'rpm4_coef': 1, 'pc_theta_kp': 0.32, 'pc_theta_kd': 0.1, 'pc_phi_kp': 0.32, 'pc_phi_kd': 0.1, 'P': 0.0, 'Q': 0.0, 'R': 0.0, 'X': 0.0, 'Y': 0.0, 'Z': 9.997891075702396, 'Phi': 0.0, 'The': 0.0, 'Psi': 0.0, 'U': 0.0, 'V': 0.0, 'W': -0.09329022699995057, 'RPM1': 3513.649870004766, 'RPM2': 3513.649870004766, 'RPM3': 3513.649870004766, 'RPM4': 3513.649870004766, 'simulation_time': 0.05, 'time': 0.04, 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487263.1179338, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487263.1189337, 'TIMING_fmu_model_response_rpms_timestamp': 1624487263.1439323, 'action_mode': 'normal'}
{'action_mode': 'normal', 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487263.148947, 'time': 0.05, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487263.149934}
------
time: 0.05
action_mode: normal
last_path_input {'path_x': 0, 'path_y': 0, 'path_z': 3, 'path_psi': 0}
current_path_input {'path_x': 0, 'path_y': 0, 'path_z': 3, 'path_psi': 0}
self.failure_time: None
self.failure_mode: None
----------
result:
{'phi_kp': 2, 'phi_ki': 1.1, 'phi_kd': 1.2, 'theta_kp': 2, 'theta_ki': 1.1, 'theta_kd': 1.2, 'psi_kp': 4, 'psi_ki': 0.5, 'psi_kd': 3.5, 'alt_kp': 2, 'alt_ki': 1.1, 'alt_kd': 3.3, 'path_x': 0, 'path_y': 0, 'path_z': 3, 'path_psi': 0, 'rpm1_coef': 1, 'rpm2_coef': 1, 'rpm3_coef': 1, 'rpm4_coef': 1, 'pc_theta_kp': 0.32, 'pc_theta_kd': 0.1, 'pc_phi_kp': 0.32, 'pc_phi_kd': 0.1, 'P': 0.0, 'Q': 0.0, 'R': 0.0, 'X': 0.0, 'Y': 0.0, 'Z': 9.996821803236182, 'Phi': 0.0, 'The': 0.0, 'Psi': 0.0, 'U': 0.0, 'V': 0.0, 'W': -0.12101244920344366, 'RPM1': 3447.153361177631, 'RPM2': 3447.153361177631, 'RPM3': 3447.153361177631, 'RPM4': 3447.153361177631, 'simulation_time': 0.06, 'time': 0.05, 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487263.148947, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487263.149934, 'TIMING_fmu_model_response_rpms_timestamp': 1624487263.187932, 'action_mode': 'normal'}
{'action_mode': 'normal', 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487263.192932, 'time': 0.06, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487263.1939356}
------
...
------
time: 14.98
action_mode: normal
last_path_input {'path_x': 7, 'path_y': -1, 'path_z': 3, 'path_psi': 0}
current_path_input {'path_x': 7, 'path_y': -1, 'path_z': 3, 'path_psi': 0}
self.failure_time: None
self.failure_mode: None
----------
result:
{'phi_kp': 2, 'phi_ki': 1.1, 'phi_kd': 1.2, 'theta_kp': 2, 'theta_ki': 1.1, 'theta_kd': 1.2, 'psi_kp': 4, 'psi_ki': 0.5, 'psi_kd': 3.5, 'alt_kp': 2, 'alt_ki': 1.1, 'alt_kd': 3.3, 'path_x': 7, 'path_y': -1, 'path_z': 3, 'path_psi': 0, 'rpm1_coef': 1, 'rpm2_coef': 1, 'rpm3_coef': 1, 'rpm4_coef': 1, 'pc_theta_kp': 0.32, 'pc_theta_kd': 0.1, 'pc_phi_kp': 0.32, 'pc_phi_kd': 0.1, 'P': -0.020585681123874957, 'Q': 0.2401197255752603, 'R': 0.009584998514792142, 'X': 7.186899376236455, 'Y': -0.9998190858752474, 'Z': 2.792348938136011, 'Phi': -0.006141011254418462, 'The': -0.040439363114357356, 'Psi': 0.0060671389606713816, 'U': -0.2731235989509231, 'V': -0.05774760443364816, 'W': 0.11032208700795355, 'RPM1': 4150.693128343732, 'RPM2': 4146.395255089921, 'RPM3': 4146.982342818612, 'RPM4': 4139.05740956209, 'simulation_time': 14.99, 'time': 14.98, 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487332.5760095, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487332.5780375, 'TIMING_fmu_model_response_rpms_timestamp': 1624487332.6130092, 'action_mode': 'normal'}
{'action_mode': 'normal', 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487332.6180093, 'time': 14.99, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487332.6200118}
------
time: 14.99
action_mode: normal
last_path_input {'path_x': 7, 'path_y': -1, 'path_z': 3, 'path_psi': 0}
current_path_input {'path_x': 7, 'path_y': -1, 'path_z': 3, 'path_psi': 0}
self.failure_time: None
self.failure_mode: None
----------
result:
{'phi_kp': 2, 'phi_ki': 1.1, 'phi_kd': 1.2, 'theta_kp': 2, 'theta_ki': 1.1, 'theta_kd': 1.2, 'psi_kp': 4, 'psi_ki': 0.5, 'psi_kd': 3.5, 'alt_kp': 2, 'alt_ki': 1.1, 'alt_kd': 3.3, 'path_x': 7, 'path_y': -1, 'path_z': 3, 'path_psi': 0, 'rpm1_coef': 1, 'rpm2_coef': 1, 'rpm3_coef': 1, 'rpm4_coef': 1, 'pc_theta_kp': 0.32, 'pc_theta_kd': 0.1, 'pc_phi_kp': 0.32, 'pc_phi_kd': 0.1, 'P': -0.019872055198971776, 'Q': 0.23782061494727635, 'R': 0.009425775708390512, 'X': 7.1841093307113555, 'Y': -1.0004036132556238, 'Z': 2.793352921809938, 'Phi': -0.006346444615824196, 'The': -0.038049039981261926, 'Psi': 0.00614733010555744, 'U': -0.2772428185444552, 'V': -0.057131789045387606, 'W': 0.11151965986289913, 'RPM1': 4150.3806036597325, 'RPM2': 4146.100918298595, 'RPM3': 4146.443379024088, 'RPM4': 4138.549365215699, 'simulation_time': 15.0, 'time': 14.99, 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487332.6180093, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487332.6200118, 'TIMING_fmu_model_response_rpms_timestamp': 1624487332.655009, 'action_mode': 'normal'}
{'action_mode': 'normal', 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487332.6600096, 'time': 15.0, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487332.66201}
------
time: 15.0
action_mode: normal
last_path_input {'path_x': 7, 'path_y': 2, 'path_z': 3, 'path_psi': 0}
current_path_input {'path_x': 7, 'path_y': 2, 'path_z': 3, 'path_psi': 0}
self.failure_time: None
self.failure_mode: None
----------
result:
{'phi_kp': 2, 'phi_ki': 1.1, 'phi_kd': 1.2, 'theta_kp': 2, 'theta_ki': 1.1, 'theta_kd': 1.2, 'psi_kp': 4, 'psi_ki': 0.5, 'psi_kd': 3.5, 'alt_kp': 2, 'alt_ki': 1.1, 'alt_kd': 3.3, 'path_x': 7, 'path_y': 2, 'path_z': 3, 'path_psi': 0, 'rpm1_coef': 0.75, 'rpm2_coef': 1, 'rpm3_coef': 1, 'rpm4_coef': 1, 'pc_theta_kp': 0.32, 'pc_theta_kd': 0.1, 'pc_phi_kp': 0.32, 'pc_phi_kd': 0.1, 'P': 0.1463594016190184, 'Q': 0.40220798028942856, 'R': 0.01995623831696358, 'X': 7.181282944598681, 'Y': -1.0009823557390227, 'Z': 2.794329612922215, 'Phi': -0.0057990843618589616, 'The': -0.03493124079690231, 'Psi': 0.0062696461946668254, 'U': -0.2811782703661765, 'V': -0.0564243140768662, 'W': 0.10265251412496708, 'RPM1': 3116.738971551912, 'RPM2': 4142.098309144044, 'RPM3': 4140.933253335456, 'RPM4': 4141.675926661159, 'simulation_time': 15.01, 'time': 15.0, 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487332.6600096, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487332.66201, 'TIMING_fmu_model_response_rpms_timestamp': 1624487332.6950092, 'action_mode': 'normal'}
{'action_mode': 'failure_total', 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487332.7000096, 'time': 15.01, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487332.701009}
------
time: 15.01
action_mode: failure_total
last_path_input {'path_x': 7, 'path_y': 2, 'path_z': 3, 'path_psi': 0}
current_path_input {'path_x': 7, 'path_y': 2, 'path_z': 3.0, 'path_psi': 0}
self.failure_time: 15.01
self.failure_mode: failure_total
----------
result:
{'phi_kp': 2, 'phi_ki': 1.1, 'phi_kd': 1.2, 'theta_kp': 2, 'theta_ki': 1.1, 'theta_kd': 1.2, 'psi_kp': 4, 'psi_ki': 0.5, 'psi_kd': 3.5, 'alt_kp': 2, 'alt_ki': 1.1, 'alt_kd': 3.3, 'path_x': 7, 'path_y': 2, 'path_z': 3.0, 'path_psi': 0, 'rpm1_coef': 0, 'rpm2_coef': 1, 'rpm3_coef': 0, 'rpm4_coef': 1, 'pc_theta_kp': 0.32, 'pc_theta_kd': 0.1, 'pc_phi_kp': 0.32, 'pc_phi_kd': 0.1, 'P': 0.1651391919514243, 'Q': 0.42066282379778724, 'R': 0.0697450057987751, 'X': 7.178430814313147, 'Y': -1.0015570066052015, 'Z': 2.7950562343683325, 'Phi': -0.004171839364986727, 'The': -0.030733690252890165, 'Psi': 0.006678419033730824, 'U': -0.284765698636689, 'V': -0.055679613151761224, 'W': 0.05726311181628889, 'RPM1': 0.0, 'RPM2': 4138.001536468069, 'RPM3': 0.0, 'RPM4': 4144.5379801590925, 'simulation_time': 15.02, 'time': 15.01, 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487332.7000096, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487332.701009, 'TIMING_fmu_model_response_rpms_timestamp': 1624487332.7380085, 'action_mode': 'failure_total'}
{'action_mode': 'failure_total', 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487332.7430112, 'time': 15.02, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487332.7440114}
------
time: 15.02
action_mode: failure_total
last_path_input {'path_x': 7, 'path_y': 2, 'path_z': 3, 'path_psi': 0}
current_path_input {'path_x': 7, 'path_y': 2, 'path_z': 2.9925, 'path_psi': 0}
self.failure_time: 15.01
self.failure_mode: failure_total
----------
result:
{'phi_kp': 2, 'phi_ki': 1.1, 'phi_kd': 1.2, 'theta_kp': 2, 'theta_ki': 1.1, 'theta_kd': 1.2, 'psi_kp': 4, 'psi_ki': 0.5, 'psi_kd': 3.5, 'alt_kp': 2, 'alt_ki': 1.1, 'alt_kd': 3.3, 'path_x': 7, 'path_y': 2, 'path_z': 2.9925, 'path_psi': 0, 'rpm1_coef': 0, 'rpm2_coef': 1, 'rpm3_coef': 0, 'rpm4_coef': 1, 'pc_theta_kp': 0.32, 'pc_theta_kd': 0.1, 'pc_phi_kp': 0.32, 'pc_phi_kd': 0.1, 'P': 0.1642564036561144, 'Q': 0.42219548607138874, 'R': 0.12374232687868635, 'X': 7.175563287960386, 'Y': -1.0021296696410011, 'Z': 2.7953019659720635, 'Phi': -0.002550785668886265, 'The': -0.026517321147799502, 'Psi': 0.007632105429845442, 'U': -0.28776438309177804, 'V': -0.05501928255769597, 'W': 0.007918496546107014, 'RPM1': 0.0, 'RPM2': 4134.053827459895, 'RPM3': 0.0, 'RPM4': 4146.502710746571, 'simulation_time': 15.030000000000001, 'time': 15.02, 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487332.7430112, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487332.7440114, 'TIMING_fmu_model_response_rpms_timestamp': 1624487332.7860107, 'action_mode': 'failure_total'}
{'action_mode': 'failure_total', 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487332.7900093, 'time': 15.030000000000001, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487332.7920098}
------
time: 15.030000000000001
action_mode: failure_total
last_path_input {'path_x': 7, 'path_y': 2, 'path_z': 3, 'path_psi': 0}
current_path_input {'path_x': 7, 'path_y': 2, 'path_z': 2.984999999999999, 'path_psi': 0}
self.failure_time: 15.01
self.failure_mode: failure_total
----------
result:
{'phi_kp': 2, 'phi_ki': 1.1, 'phi_kd': 1.2, 'theta_kp': 2, 'theta_ki': 1.1, 'theta_kd': 1.2, 'psi_kp': 4, 'psi_ki': 0.5, 'psi_kd': 3.5, 'alt_kp': 2, 'alt_ki': 1.1, 'alt_kd': 3.3, 'path_x': 7, 'path_y': 2, 'path_z': 2.984999999999999, 'path_psi': 0, 'rpm1_coef': 0, 'rpm2_coef': 1, 'rpm3_coef': 0, 'rpm4_coef': 1, 'pc_theta_kp': 0.32, 'pc_theta_kd': 0.1, 'pc_phi_kp': 0.32, 'pc_phi_kd': 0.1, 'P': 0.16205734224584376, 'Q': 0.42491923955237954, 'R': 0.17770928013007306, 'X': 7.172682544723987, 'Y': -1.0027011618663721, 'Z': 2.795064683312763, 'Phi': -0.0009546148859906605, 'The': -0.0222800988902818, 'Psi': 0.009132425211798119, 'U': -0.2901691638398528, 'V': -0.05443919423709758, 'W': -0.04148499385067225, 'RPM1': 0.0, 'RPM2': 4130.244363921911, 'RPM3': 0.0, 'RPM4': 4147.632236740604, 'simulation_time': 15.040000000000001, 'time': 15.030000000000001, 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487332.7900093, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487332.7920098, 'TIMING_fmu_model_response_rpms_timestamp': 1624487332.8260102, 'action_mode': 'failure_total'}
{'action_mode': 'failure_total', 'TIMING_fd_model_actoin_mode_publish_timestamp': 1624487332.830009, 'time': 15.040000000000001, 'TIMING_fmu_model_recieve_action_mode_timestamp': 1624487332.83201}
------
Results¶
Let's examine the results of the experiment by plotting some inputs and outputs.
In [109]:
import pandas as pd
import numpy as np
step_size = 0.01
log_path = "experiment_2021-06-23_15-33-13-failure-paper-results.csv"
print(log_path)
pd.options.display.float_format = '{:.5f}'.format
data = pd.read_csv(log_path)
if len(data["Z"][data["Z"] <= 0]) > 0:
crash_time = data["simulation_time"].iloc[data["Z"][data["Z"] <= 0].index[0]]
crash_x = data["X"].iloc[data["Z"][data["Z"] <= 0].index[0]]
crash_y = data["Y"].iloc[data["Z"][data["Z"] <= 0].index[0]]
data = data[:int((crash_time)/step_size)]
else:
crash_time = None
failure_time = data["simulation_time"].iloc[data["rpm1_coef"][data["rpm1_coef"] < 1.0].index[0]]
failure_detection_time = data["simulation_time"].iloc[data["rpm1_coef"][data["rpm1_coef"] == 0].index[0]]
failure_detection_x = data["X"].iloc[data["rpm1_coef"][data["rpm1_coef"] == 0].index[0]]
failure_detection_y = data["Y"].iloc[data["rpm1_coef"][data["rpm1_coef"] == 0].index[0]]
failure_detection_z = data["Z"].iloc[data["rpm1_coef"][data["rpm1_coef"] == 0].index[0]]
data = data[int(0/step_size):int(20/step_size)]
print(f"failure time: {failure_time}")
print(f"failure_detection_time: {failure_detection_time}")
print(f"crash_time: {crash_time}")
print(len(data))
print(data.columns)
experiment_2021-06-23_15-33-13-failure-paper-results.csv
failure time: 15.01
failure_detection_time: 15.02
crash_time: 19.51
1951
Index(['timestamp', 'action_mode', 'P', 'Q', 'R', 'Phi', 'The', 'Psi', 'U',
'V', 'W', 'X', 'Y', 'Z', 'RPM1', 'RPM2', 'RPM3', 'RPM4',
'simulation_time', 'time',
'TIMING_fd_model_actoin_mode_publish_timestamp',
'TIMING_fmu_model_recieve_action_mode_timestamp',
'TIMING_fmu_model_response_rpms_timestamp', 'phi_kp', 'phi_ki',
'phi_kd', 'theta_kp', 'theta_ki', 'theta_kd', 'psi_kp', 'psi_ki',
'psi_kd', 'alt_kp', 'alt_ki', 'alt_kd', 'pc_phi_kp', 'pc_phi_kd',
'pc_theta_kp', 'pc_theta_kd', 'path_x', 'path_y', 'path_z', 'path_psi',
'rpm1_coef', 'rpm2_coef', 'rpm3_coef', 'rpm4_coef'],
dtype='object')
Plot the simulation outputs
In [110]:
font = {'size' : 12}
import matplotlib
from matplotlib import pyplot as plt
matplotlib.rc('font', **font)
keys = [["Phi"], ["The"], ["Psi","path_psi"], ["X", "path_x"], ["Y", "path_y"], ["Z", "path_z"],
["RPM1"], ["RPM2"], ["RPM3"], ["RPM4"],
["rpm1_coef", "rpm2_coef","rpm3_coef", "rpm4_coef"],
["U"], ["V"], ["W"],
["P"], ["Q"], ["R"]]
#+ [[x] for x in ['phi_kp', 'phi_ki', 'phi_kd',
#'theta_kp', 'theta_ki', 'theta_kd', 'psi_kp', 'psi_ki', 'psi_kd',
#'alt_kp', 'alt_ki', 'alt_kd', 'pc_phi_kp', 'pc_phi_kd', 'pc_theta_kp',
#'pc_theta_kd']]
fig, axs = plt.subplots(len(keys), figsize=(8, len(keys) * 1.4 ), tight_layout=True)
for i,ks in enumerate(keys):
axs[i].set_ylabel("\n".join(ks))
for k in ks:
axs[i].plot(data["simulation_time"], data[k])
if crash_time is not None:
axs[i].axvline(x=crash_time, color="red", linewidth=0.5, label="xx")
# axs[i].axvline(x=failure_time, color="purple", linewidth=0.5)
axs[i].axvline(x=failure_detection_time, color="green", linewidth=0.5)
axs[i].grid(True)
locs = axs[i].get_xticks()
axs[i].set_xlim((-1, crash_time+1))
axs[i].set_xticks(np.append(np.arange(0, 19, 2, dtype=np.int), crash_time), minor=True)
fig.align_labels()
plt.xlabel('time (s)')
plt.show()
plt.plot(data["path_x"], data["path_y"], marker = 'o')
plt.plot(data["X"], data["Y"])
plt.xlabel("x")
plt.ylabel("y")
plt.show()
plt.plot(data["path_x"], data["path_z"], marker = 'o')
plt.plot(data["X"], data["Z"])
plt.xlabel("x")
plt.ylabel("z")
plt.show()
plt.plot(data["path_y"], data["path_z"], marker = 'o')
plt.plot(data["Y"], data["Z"])
plt.xlabel("y")
plt.ylabel("z")
plt.show()
ax = plt.axes(projection='3d')
ax.plot3D(data["path_x"], data["path_y"], data["path_z"], marker = 'o')
ax.plot3D(data["X"], data["Y"], data["Z"])
#ax.scatter3D(xdata, ydata, zdata, c=zdata, cmap='Greens');
Out[110]:
[<mpl_toolkits.mplot3d.art3d.Line3D at 0x1fe66c85088>]
In [98]:
font = {'size' : 12}
import matplotlib
from matplotlib import pyplot as plt
# matplotlib.rc('font', **font)
fig, axs = plt.subplots(3, figsize=(6, 3 * 2 ), tight_layout=True)
axs[0].set_ylabel("X (m)")
axs[0].plot(data["simulation_time"], data['X'])
axs[0].plot(data["simulation_time"], data['path_x'])
axs[1].set_ylabel("Y (m)")
axs[1].plot(data["simulation_time"], data['Y'])
axs[1].plot(data["simulation_time"], data['path_y'])
axs[2].set_ylabel("Z (m)")
axs[2].plot(data["simulation_time"], data['Z'])
axs[2].plot(data["simulation_time"], data['path_z'])
for i in range(len(axs)):
axs[i].axvline(x=crash_time, color="red", linewidth=1.2, ls='--')
# axs[i].axvline(x=failure_time, color="purple", linewidth=0.5)
axs[i].axvline(x=failure_detection_time, color="green", linewidth=1.2)
axs[i].grid(True)
# locs = axs[i].get_xticks()
axs[i].set_xlim((-1, crash_time+1))
axs[i].set_xticks(np.append(np.arange(0, 19, 2, dtype=np.int), crash_time), minor=True)
axs[0].legend(['X', 'Reference X'], ncol = 2)
axs[1].legend(['Y', 'Reference Y'], ncol = 2)
axs[2].legend(['Z', 'Reference Z'], ncol = 2)
fig.align_labels()
plt.xlabel('time (s)')
plt.savefig('results_positions.pdf')
plt.show()
In [99]:
keys = ["RPM1", "RPM2", "RPM3", "RPM4"]
fig, axs = plt.subplots(len(keys), figsize=(6, len(keys) * 2 ), tight_layout=True)
for i,ks in enumerate(keys):
axs[i].set_ylabel(ks)
axs[i].plot(data["simulation_time"], data[ks])
axs[i].axvline(x=crash_time, color="red", linewidth=1.2, ls='--')
axs[i].axvline(x=failure_detection_time, color="green", linewidth=1.2)
axs[i].grid(True)
locs = axs[i].get_xticks()
axs[i].set_xlim((-1, crash_time+1))
axs[i].set_xticks(np.append(np.arange(0, 19, 2, dtype=np.int), crash_time), minor=True)
fig.align_labels()
plt.xlabel('time (s)')
plt.savefig('results_rpms.pdf')
plt.show()
In [100]:
# ["Phi"], ["The"], ["Psi","path_psi"]
fig, axs = plt.subplots(3, figsize=(6, 3 * 2 ), tight_layout=True)
axs[0].set_ylabel("Phi (rad)")
axs[0].plot(data["simulation_time"], data['Phi'])
axs[1].set_ylabel("Theta (rad)")
axs[1].plot(data["simulation_time"], data['The'])
axs[2].set_ylabel("Psi (rad)")
axs[2].plot(data["simulation_time"], data['Psi'])
axs[2].plot(data["simulation_time"], data['path_psi'])
for i in range(len(axs)):
axs[i].axvline(x=crash_time, color="red", linewidth=1.2, ls='--')
# axs[i].axvline(x=failure_time, color="purple", linewidth=0.5)
axs[i].axvline(x=failure_detection_time, color="green", linewidth=1.2)
axs[i].grid(True)
# locs = axs[i].get_xticks()
axs[i].set_xlim((-1, crash_time+1))
axs[i].set_xticks(np.append(np.arange(0, 19, 2, dtype=np.int), crash_time), minor=True)
axs[2].legend(['Psi', 'Reference Psi'], ncol = 2)
fig.align_labels()
plt.xlabel('time (s)')
plt.savefig('results_attitudes.pdf')
plt.show()
In [106]:
from matplotlib.pyplot import figure
figure(figsize=(8, 6), dpi=100)
ax = plt.axes(projection='3d')
ax.plot3D(data["X"], data["Y"], data["Z"])
ax.plot3D(data["path_x"], data["path_y"], data["path_z"])
ax.scatter3D(data["X"][0], data["Y"][0], data["Z"][0], color='blue', marker="x", s=60)
ax.scatter3D([failure_detection_x], [failure_detection_y], [failure_detection_z], color='green', marker="s", s=60)
ax.scatter3D([crash_x], [crash_y], [0], color='red', marker="o", s=60)
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.legend(["Actual path", "Reference path"])
plt.savefig('results_3d_position.pdf')
In [ ]: