Major rework using rocketpy

This commit is contained in:
dario 2024-06-07 15:01:16 +02:00
parent 1b06db4152
commit 382cb9aad4
25 changed files with 13173 additions and 102 deletions

3
.vscode/launch.json vendored
View File

@ -9,7 +9,8 @@
"type": "debugpy",
"request": "launch",
"program": "testing.py",
"console": "integratedTerminal"
"console": "integratedTerminal",
"justMyCode": false
}
]
}

5001
dummy.csv Normal file

File diff suppressed because it is too large Load Diff

5001
dummy2.csv Normal file

File diff suppressed because it is too large Load Diff

2646
nominal_wind.csv Normal file

File diff suppressed because it is too large Load Diff

36
rework_test.py Normal file
View File

@ -0,0 +1,36 @@
import cProfile
import numpy as np
import tqdm
from spatz.simulation import Simulation
from spatz.simulations.rocketpy import RocketPyCSV
from spatz.sensors.pressure.ms5611 import MS5611
from spatz.sensors.imu.wsen_isds import WSEN_ISDS_ACC, WSEN_ISDS_GYRO
from spatz.sensors.imu.h3lis100dl import H3LIS100DL
simulation = Simulation().load(RocketPyCSV('nominal_wind.csv'))
barometer = simulation.add_sensor(MS5611)
# Orientation matrix of the WSEN-ISDS IMU on the rocket.
orientation = np.array([
[ 1, 0, 0],
[ 0, 0, -1],
[ 0, -1, 0]
])
accelerometer = simulation.add_sensor(WSEN_ISDS_ACC, orientation=orientation)
gyroscope = simulation.add_sensor(WSEN_ISDS_GYRO)
high_g = simulation.add_sensor(H3LIS100DL)
for i in tqdm.tqdm(range(5000)):
simulation.advance(0.1)
pressure = barometer()
acc = accelerometer()
omegas = gyroscope()
g = high_g()
df = simulation.get_logger().get_dataframe()
df.to_csv('dummy2.csv')

View File

@ -4,43 +4,7 @@ import pandas as pd
from typing import Any, Tuple, List
from numpy.typing import ArrayLike
from abc import abstractmethod
class Advanceable:
def __init__(self) -> None:
self.reset()
def step(self, dt: float):
"""Advances the simulation data in time.
Args:
dt (float): The step in time to make.
"""
self.__t += dt
self._on_step(dt)
def reset(self):
"""
Reset the Avanceable object to its initial state.
"""
self.__t = 0
self._on_reset()
@abstractmethod
def _on_step(self, dt: float):
pass
@abstractmethod
def _on_reset(self):
pass
def get_time(self) -> float:
"""
Returns:
float: Returns the current time of the Advanceable.
"""
return self.__t
from spatz.simulations.advanceable import Advanceable
class Logger(Advanceable):

View File

@ -1,6 +1,6 @@
from spatz.sensors.sensor import Sensor
from spatz.sensors.gps import GPS
from spatz.sensors.imu import Accelerometer, Gyroscope, IMU
from spatz.sensors.imu import Accelerometer, Gyroscope, IMU, CoordSystem
from spatz.sensors.pressure import PressureSensor
from spatz.sensors.temperature import TemperatureSensor
from spatz.sensors.compound import CompoundSensor

View File

@ -1,3 +1,3 @@
from spatz.sensors.imu.accelerometer import Accelerometer
from spatz.sensors.imu.accelerometer import Accelerometer, CoordSystem
from spatz.sensors.imu.gyroscope import Gyroscope
from spatz.sensors.imu.imu import IMU

View File

@ -1,11 +1,13 @@
import numpy as np
from typing import List
from numpy.typing import ArrayLike
from enum import Enum
from numpy.typing import ArrayLike, NDArray
from spatz.sensors import Sensor
from spatz.transforms import Transform
from spatz.dataset import Dataset
from spatz.simulations.data_source import DataSource
from spatz.logger import Logger
@ -18,33 +20,61 @@ __all__=[
g = 9.81
class CoordSystem:
LEFT_HANDED = 0,
RIGHT_HANDED = 0,
class Accelerometer(Sensor):
def __init__(self, dataset: Dataset, logger: Logger, offset: float = 0, transforms: List[Transform] = []):
def __init__(
self, dataset: DataSource,
logger: Logger,
coord_system: CoordSystem = CoordSystem.RIGHT_HANDED,
orientation: NDArray = np.identity(3),
offset: float = 0,
transforms: List[Transform] = []):
"""_summary_
Args:
dataset (Dataset): A dataset object to fetch data from.
logger (Logger): A logger object to write data to.
coord_system (CoordSystem, optional): The type of coordinate system to use. Defaults to CoordSystem.RIGHT_HANDED.
orientation (NDArray, optional): The orientation of the sensor inside the spacecraft. Defaults to np.identity(3).
offset (float, optional): The offset of the sensor from the origin around which the object rotates. Defaults to 0.
transforms (List[Transform], optional): A list of transformations applied to the sensor measurements. Defaults to [].
"""
super().__init__(dataset, logger, transforms)
assert orientation.shape == (3, 3), 'Orientation has to be a 3x3 matrix.'
self._offset = np.array([offset, 0, 0])
self._coord_system = coord_system
self._orientation = orientation
def _get_data(self) -> ArrayLike | float:
acc = self._dataset.get_acceleration(frame='FL')
acc = self._dataset.get_acceleration('global')
self._logger.write('FL_x', acc[0], self._get_name())
self._logger.write('FL_y', acc[1], self._get_name())
self._logger.write('FL_z', acc[2], self._get_name())
# self._logger.write('global_ax', acc[0], self._get_name())
# self._logger.write('global_ay', acc[1], self._get_name())
# self._logger.write('global_az', acc[2], self._get_name())
# Convert FL to body
mat = self._dataset.launch_rail_to_body()
acc = mat @ acc
acc += mat @ np.array([0, 0, g])
acc = self._dataset.global_to_local() @ acc
# acc += mat @ np.array([0, 0, g])
self._logger.write('B_x', acc[0], self._get_name())
self._logger.write('B_y', acc[1], self._get_name())
self._logger.write('B_z', acc[2], self._get_name())
# self._logger.write('local_x', acc[0], self._get_name())
# self._logger.write('local_y', acc[1], self._get_name())
# self._logger.write('local_z', acc[2], self._get_name())
# Flip axes to sensor's perspective.
acc *= -1
# Flip axes to sensor's perspective. Spatz uses a right-handed coordinate system.
#if self._coord_system == CoordSystem.LEFT_HANDED:
# acc[1] *= -1
# Rotate the acceleration vector to accomodate the accelerometer's orientation on the rocket.
acc = self._orientation @ acc
# Add the effects of the imu's offset.
omega = self._dataset.get_angular_velocities()
omega = self._dataset.get_angular_velocity()
acc += (np.cross(omega, self._offset) + np.cross(omega, np.cross(omega, self._offset)))
return acc

View File

@ -0,0 +1,46 @@
import numpy as np
from typing import List, AnyStr
from numpy.typing import ArrayLike
from spatz.dataset import ArrayLike, Dataset
from spatz.logger import ArrayLike, Logger
from spatz.sensors import IMU, Accelerometer, Gyroscope, CoordSystem
from spatz.transforms import Transform, GaussianNoise
class BMI088(IMU):
def __init__(self, dataset: Dataset, logger: Logger, orientation = np.identity(3), offset=0, transforms: List[Transform] = []):
acc = BMI088Acc(dataset, logger, orientation, offset, transforms)
gyro = BMI088Gyro(dataset, logger, offset, transforms)
super().__init__(dataset, logger, acc, gyro, transforms)
class BMI088Gyro(Gyroscope):
def __init__(self, dataset: Dataset, logger: Logger, offset: float = 0, transforms: List[Transform] = []):
super().__init__(dataset, logger, offset, transforms)
def _get_name(self) -> AnyStr:
return 'BMI088'
def _get_data(self) -> ArrayLike:
rots = self._dataset.fetch_values(['roll_l', 'pitch_l', 'yaw_l'])
return rots
def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
return x
class BMI088Acc(Accelerometer):
def __init__(self, dataset: Dataset, logger: Logger, orientation = np.identity(3), offset: float = 0, transforms: List[Transform] = []):
super().__init__(dataset, logger, CoordSystem.RIGHT_HANDED, orientation, offset, transforms)
self.__noise = GaussianNoise(0, 0.05)
def _get_name(self) -> AnyStr:
return 'BMI088'
def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
return self.__noise(0, x)

View File

@ -4,17 +4,18 @@ from typing import List
from spatz.sensors import Sensor
from spatz.transforms import Transform
from spatz.dataset import Dataset
from spatz.simulations.data_source import DataSource
from spatz.logger import Logger
class Gyroscope(Sensor):
def __init__(self, dataset: Dataset, logger: Logger, offset: float = 0, transforms: List[Transform] = []):
def __init__(self, dataset: DataSource, logger: Logger, offset: float = 0, transforms: List[Transform] = []):
super().__init__(dataset, logger, transforms)
self._offset = offset
def _get_data(self) -> ArrayLike | float:
# Rotation in rad/sec
x = self._dataset.get_angular_velocities()
x = self._dataset.get_angular_velocity()
return x

View File

@ -0,0 +1,30 @@
import numpy as np
from typing import List, AnyStr
from numpy.core.numeric import identity as identity
from numpy.typing import ArrayLike, NDArray
from spatz.dataset import Dataset
from spatz.simulations.data_source import DataSource
from spatz.logger import Logger
from spatz.sensors import Accelerometer, CoordSystem
from spatz.transforms import Transform
class H3LIS100DL(Accelerometer):
def __init__(self, dataset: DataSource, logger: Logger, orientation=np.identity(3), offset: float = 0, transforms: List[Transform] = []):
super().__init__(dataset, logger, CoordSystem.RIGHT_HANDED, orientation, offset, transforms)
def _get_name(self) -> AnyStr:
return "H3LIS100DL"
def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
# The sensor only measures values between -100g and +100g and in g/LSB.
g = 9.81
x = np.floor(np.clip(x, -100*g, +100*g) / g)
return x

View File

@ -3,8 +3,9 @@ import numpy as np
from typing import AnyStr, List
from numpy.typing import ArrayLike
from spatz.sensors import Accelerometer, Gyroscope, IMU
from spatz.sensors import Accelerometer, Gyroscope, IMU, CoordSystem
from spatz.transforms import Transform, GaussianNoise
from spatz.simulations.data_source import DataSource
from spatz.dataset import Dataset
from spatz.logger import Logger
@ -14,8 +15,8 @@ class WSEN_ISDS(IMU):
class WSEN_ISDS_ACC(Accelerometer):
def __init__(self, dataset: Dataset, logger: Logger, offset: float, transforms: List[Transform] = []):
super().__init__(dataset, logger, offset, transforms)
def __init__(self, dataset: DataSource, logger: Logger, orientation=np.identity(3), offset=0, transforms: List[Transform] = []):
super().__init__(dataset, logger, CoordSystem.LEFT_HANDED, orientation, offset, transforms)
self.__variance = 0.05
self.__noise = GaussianNoise(np.zeros(3), np.identity(3) * self.__variance)
@ -35,11 +36,15 @@ class WSEN_ISDS_ACC(Accelerometer):
self._logger.write('acc_y_noise', noise[1], self._get_name())
self._logger.write('acc_z_noise', noise[2], self._get_name())
# The WSEN-ISDS accelerometer only measures acceleration between -16g and 16g.
g = 9.81
y = np.clip(y, -16*g, +16*g)
return y
class WSEN_ISDS_GYRO(Gyroscope):
def __init__(self, dataset: Dataset, logger: Logger, offset: float, transforms: List[Transform] = []):
def __init__(self, dataset: Dataset, logger: Logger, offset=0, transforms: List[Transform] = []):
super().__init__(dataset, logger, offset, transforms)
def _get_name(self) -> AnyStr:
@ -51,4 +56,8 @@ class WSEN_ISDS_GYRO(Gyroscope):
# TODO: Noise model.
self._log('ox', x[0])
self._log('oy', x[1])
self._log('oz', x[2])
return x

View File

@ -3,12 +3,13 @@ from numpy.typing import ArrayLike
from spatz.sensors import PressureSensor
from spatz.dataset import Dataset, Phase
from spatz.simulations.data_source import DataSource
from spatz.logger import Logger
from spatz.transforms import GaussianNoise, Transform, ProportionalGaussian
class MS5611(PressureSensor):
def __init__(self, dataset: Dataset, logger: Logger, transforms: List[Transform] = [], ts_effects=True):
def __init__(self, dataset: DataSource, logger: Logger, transforms: List[Transform] = [], ts_effects=True):
super().__init__(dataset, logger, transforms, ts_effects)
# Noise model obtained by a test flight using this sensor.

View File

@ -6,11 +6,12 @@ from typing import List
from spatz.sensors import Sensor
from spatz.logger import Logger
from spatz.dataset import Dataset
from spatz.simulations.data_source import DataSource
from spatz.transforms import Transform
class PressureSensor(Sensor):
def __init__(self, dataset: Dataset, logger: Logger, transforms: List[Transform] = [], ts_effects=True, delay=0.0):
def __init__(self, dataset: DataSource, logger: Logger, transforms: List[Transform] = [], ts_effects=True, delay=0.0):
"""_summary_
Args:
@ -28,7 +29,7 @@ class PressureSensor(Sensor):
self._ts_effects = active
def _get_data(self) -> float:
x = self._dataset.get_pressure()
x = self._dataset.get_static_pressure()
if self._ts_effects:
# Pre-defined constants.
@ -36,16 +37,16 @@ class PressureSensor(Sensor):
sigma = 40
# How far away from transsonic speed (mach 1) are we?
vvec = self._dataset.get_velocity()
vvec = self._dataset.get_velocity('global')
dv = np.abs(np.linalg.norm(vvec) - self._dataset.get_speed_of_sound())
# Model transsonic effects by a peak at mach 1 which decays the further we are away from it.
ts_eff = _p * math.exp(-0.5* (dv / sigma)**2 ) / (sigma * math.sqrt(2*math.pi))
# Log the values for the transsonic effect.
self._logger.write('ts_effects', ts_eff, domain=self._get_name())
self._logger.write('mach_no', self._dataset.get_mach_number(), domain='mach')
self._logger.write('speedofsound', self._dataset.get_speed_of_sound(), domain='mach')
self._log('ts_effects', ts_eff)
self._log('mach_no', self._dataset.get_mach_number())
self._log('speedofsound', self._dataset.get_speed_of_sound())
x = x + ts_eff

View File

@ -9,17 +9,24 @@ from pandas import NA
from spatz.transforms import *
from spatz.logger import *
from spatz.dataset import *
from spatz.simulations.data_source import DataSource
class Sensor:
def __init__(self, dataset: Dataset, logger: Logger, transforms: List[Transform] = [], min_value=-np.inf, max_value=np.inf):
def __init__(
self,
dataset: DataSource,
logger: Logger,
transforms: List[Transform] = [],
min_value=-np.inf,
max_value=np.inf):
self._dataset = dataset
self._logger = logger
self._transforms = transforms
self._min_value = min_value
self._max_value = max_value
def set_dataset(self, dataset: Dataset):
def set_dataset(self, dataset: DataSource):
self._dataset = dataset
def set_logger(self, logger: Logger):

View File

@ -1 +1 @@
from temperature import TemperatureSensor
from spatz.sensors.temperature.temperature import TemperatureSensor

View File

@ -8,7 +8,7 @@ from spatz.transforms import Transform
class MS5611Temperature(TemperatureSensor):
def __init__(self, dataset: Dataset, transforms: List[Transform] = ...):
def __init__(self, dataset: Dataset, transforms: List[Transform] = []):
super().__init__(dataset, transforms)
self.__noise = GaussianNoise(0, 0.5)

View File

@ -2,6 +2,8 @@ from typing import List
from numpy.random import normal
from tqdm import tqdm
from spatz.simulations.advanceable import Advanceable
from spatz.simulations.data_source import DataSource
from spatz.dataset import Dataset
from spatz.logger import Logger
from spatz.sensors import Sensor
@ -35,9 +37,11 @@ class UniformTimeSteps:
return self.__dt + noise
class Simulation:
class Simulation(Advanceable):
def __init__(self, time_steps=UniformTimeSteps(0.01)):
self.__dataset = None
super().__init__()
self.__data_source = None
self.__logger = None
self.__sensors: List[Sensor] = []
self.__time_steps = time_steps
@ -46,26 +50,25 @@ class Simulation:
idx = 0
# Clear all logs and reset the dataset to the first time step.
self.__dataset.reset()
self.__data_source.reset()
self.__logger.reset()
if verbose:
pbar = tqdm(total=self.__dataset.get_length())
pbar = tqdm(total=self.__data_source.get_length())
while True:
t = self.__dataset.get_time()
t = self.get_time()
dt = self.__time_steps(t)
t_ = t + dt
idx += 1
if t_ > self.__dataset.get_length():
if t_ > self.__data_source.get_length():
break
if until is not None and self.__dataset.get_phase() == until:
if until is not None and self.__data_source.get_phase() == until:
break
self.__dataset.step(dt)
self.__logger.step(dt)
self.advance(dt)
if verbose:
pbar.update(dt)
@ -75,31 +78,26 @@ class Simulation:
if verbose:
pbar.close()
def advance_to(self, t: float):
t_old = self.__dataset.get_time()
self.__dataset.step(t - t_old)
self.__logger.step(t - t_old)
def _on_step(self, dt: float):
self.__data_source.advance(dt)
self.__logger.advance(dt)
def advance(self, dt: float):
self.__dataset.step(dt)
self.__logger.step(dt)
def get_dataset(self) -> Dataset:
return self.__dataset
def get_logger(self) -> Logger:
return self.__logger
def load(self, path: str):
self.__dataset = Dataset(path)
def load(self, source: DataSource):
self.__data_source = source
self.__logger = Logger()
for sensor in self.__sensors:
sensor.set_dataset(self.__dataset)
sensor.set_dataset(self.__data_source)
sensor.set_logger(self.__logger)
return self
def get_data_source(self) -> DataSource:
return self.__data_source
def get_logger(self) -> Logger:
return self.__logger
def add_sensor(self, sensor, *args, **kwargs) -> Sensor:
"""Register a new sensor for this simulation. A registered sensor can be called like a function and returns
the current measurements. The class' constructor arguments have to be given aswell.
@ -112,7 +110,7 @@ class Simulation:
"""
assert issubclass(sensor, Sensor), "Expected a subclass of Sensor."
self.__sensors.append(sensor(self.__dataset, self.__logger, *args, **kwargs))
self.__sensors.append(sensor(self.__data_source, self.__logger, *args, **kwargs))
return self.__sensors[-1]
@ -132,10 +130,10 @@ class Simulation:
attributes = observer_or_attributes
assert len(attributes) != 0, "Observed attributes list must be nonempty."
self.__sensors.append(Observer(self.__dataset, self.__logger, attributes))
self.__sensors.append(Observer(self.__data_source, self.__logger, attributes))
else:
observer = observer_or_attributes
self.__sensors.append(observer(self.__dataset, self.__logger))
self.__sensors.append(observer(self.__data_source, self.__logger))
return self.__sensors[-1]

View File

@ -0,0 +1,47 @@
from abc import abstractmethod
class Advanceable:
def __init__(self) -> None:
self.reset()
def advance(self, dt: float):
"""Advances the simulation data in time.
Args:
dt (float): The step in time to make.
"""
self.__t += dt
self._on_step(dt)
def advance_to(self, t: float):
"""Advances the simulation data to a new point in time.
Args:
t (float): The target point in time.
"""
assert t > self.__t, 'Advanceable can only move forward in time.'
self.advance(t - self.__t)
def reset(self):
"""
Reset the Avanceable object to its initial state.
"""
self.__t = 0
self._on_reset()
@abstractmethod
def _on_step(self, dt: float):
pass
@abstractmethod
def _on_reset(self):
pass
def get_time(self) -> float:
"""
Returns:
float: Returns the current time of the Advanceable.
"""
return self.__t

View File

@ -0,0 +1,91 @@
import pandas as pd
import numpy as np
from typing import Literal, List
from numpy.typing import NDArray
from spatz.simulations.data_source import DataSource
class CSVSource(DataSource):
def __init__(self, path: str, time_col: str, interpolation: Literal['linear']='linear') -> None:
"""A data source that extracts all its data from a csv file.
Args:
time_col (str): The name of the column that contains time data.
"""
super().__init__()
self._df = pd.read_csv(path)
self._time_col = time_col
self._idx = 0
self._interpolation = interpolation
def get_length(self) -> float:
return max(self._df[self._time_col])
def _on_reset(self):
pass
def _get_closest_idx(self, t: float) -> int:
"""Gets an index _idx_ for the dataframe _df_ such that the values at the given time _t_ are somewhere between
_idx_ and _idx+1_.
Args:
t (float): The requested time.
Returns:
int: The computed index.
"""
idx = (self._df[self._time_col] - t).abs().idxmin()
idx = idx if self._df[self._time_col].loc[idx] <= t else idx - 1
return idx
def _on_step(self, _: float):
self._idx = self._get_closest_idx(self.get_time())
def fetch_value(self, name: str, t: float | None = None, custom_interpolation=None) -> float:
"""Get a specific value from the dataframe.
Args:
name (str): The name of the value to fetch.
t (float): Allows specification of a different time instead of the current time. None for current time.
Returns:
float: Returns the requested value.
"""
idx = self._idx if t is None else self._get_closest_idx(t)
if self._interpolation == 'linear':
t_min = self._df.at[idx, self._time_col]
t_max = self._df.at[idx + 1, self._time_col]
# Sometimes no time passes in-between two samples.
if t_max == t_min:
return self._df.at[name, idx]
# Compute the weight for interpolation.
alpha = (self.get_time() - t_min) / (t_max - t_min)
if custom_interpolation is not None:
a = self._df.at[idx, name]
b = self._df.at[idx + 1, name]
return custom_interpolation(a, b, alpha)
# Interpolate linearly between the two data points.
return (1 - alpha) * self._df.at[idx, name] + alpha * self._df.at[idx + 1, name]
def fetch_values(self, names: List[str], t: float | None = None, custom_interpolation=None) -> NDArray:
"""Get specific values from the dataframe.
Args:
names (List[str]): Names of the values to get.
t (float): Allows specification of a different time instead of the current time. None for current time.
Returns:
np.array: Returns a numpy array containing the requested values in the same order as in the input list.
"""
return np.asarray([self.fetch_value(name, t, custom_interpolation) for name in names])

View File

@ -0,0 +1,74 @@
import numpy as np
from spatz.simulations.advanceable import Advanceable
from numpy.typing import NDArray
from abc import abstractmethod
from typing import Literal
from ambiance import Atmosphere
class DataSource(Advanceable):
def __init__(self) -> None:
super().__init__()
def get_speed_of_sound(self) -> float:
return Atmosphere(self.get_altitude()).speed_of_sound
def get_mach_number(self) -> float:
speed = np.linalg.norm(self.get_velocity('global'))
return speed / self.get_speed_of_sound()
@abstractmethod
def get_length(self) -> float:
pass
@abstractmethod
def get_position(self) -> NDArray:
pass
@abstractmethod
def get_velocity(self, frame: Literal['global', 'local']) -> NDArray:
pass
@abstractmethod
def get_acceleration(self, frame: Literal['global', 'local']) -> NDArray:
pass
@abstractmethod
def get_attitude(self) -> NDArray:
pass
@abstractmethod
def local_to_global(self) -> NDArray:
pass
@abstractmethod
def global_to_local(self) -> NDArray:
pass
@abstractmethod
def get_angular_velocity(self) -> NDArray:
pass
@abstractmethod
def get_static_pressure(self) -> float:
pass
@abstractmethod
def get_temperature(self) -> float:
pass
@abstractmethod
def get_longitude(self) -> float:
pass
@abstractmethod
def get_latitude(self) -> float:
pass
@abstractmethod
def get_altitude(self) -> float:
pass

View File

@ -0,0 +1,87 @@
import numpy as np
from typing import Literal
from numpy.typing import NDArray
from spatz.simulations.csv_source import CSVSource
class RocketPyCSV(CSVSource):
def __init__(self, path: str, interpolation: Literal['linear'] = 'linear') -> None:
super().__init__(path, ' Time (s)', interpolation)
def get_position(self) -> NDArray:
return self.fetch_values([' X (m)', ' Y (m)', ' Z (m)'])
def get_velocity(self, frame: Literal['global', 'local']) -> NDArray:
vel_global = self.fetch_values([' Vx (m/s)', ' Vy (m/s)', ' Vz (m/s)'])
if frame == 'global':
return vel_global
return self.global_to_local() @ vel_global
def get_acceleration(self, frame: Literal['global', 'local']) -> NDArray:
acc_global = self.fetch_values([' Ax (m/s²)', ' Ay (m/s²)', ' Az (m/s²)'])
if frame == 'global':
return acc_global
return self.global_to_local() @ acc_global
def get_attitude(self) -> NDArray:
t_min = self._df.at[self._idx, self._time_col]
t_max = self._df.at[self._idx + 1, self._time_col]
def slerp(a, b, alpha):
theta = np.arccos(np.clip(np.dot(a, b), -1, 1))
if np.isclose(theta, 0):
return a
return (a * np.sin((1-alpha) * theta) + b * np.sin(alpha * theta)) / np.sin(theta)
qa = np.array([self._df.at[self._idx, ' e0'], self._df.at[self._idx, ' e1'], self._df.at[self._idx, ' e2'], self._df.at[self._idx, ' e3']])
qb = np.array([self._df.at[self._idx+1, ' e0'], self._df.at[self._idx+1, ' e1'], self._df.at[self._idx+1, ' e2'], self._df.at[self._idx+1, ' e3']])
alpha = (self.get_time() - t_min) / (t_max - t_min)
return slerp(qa, qb, alpha)
def global_to_local(self) -> NDArray:
quat = self.get_attitude()
e0, e1, e2, e3 = quat[0], quat[1], quat[2], quat[3]
# Taken from:
# https://docs.rocketpy.org/en/latest/technical/equations_of_motion.html
mat = np.array([
[e0**2 + e1**2 - e2**2 - e3**2, 2*(e1*e2+e0*e3), 2*(e1*e3 - e0*e2)],
[2*(e1*e2 - e0*e3), e0**2 - e1**2 + e2**2 - e3**2, 2*(e2*e3 + e0*e1)],
[2*(e1*e3 + e0*e2), 2*(e2*e3 - e0*e1), e0**2 - e1**2 - e2**2 + e3**2]
])
return mat
def local_to_global(self) -> NDArray:
return self.global_to_local().T
def get_angular_velocity(self) -> NDArray:
return self.fetch_values([' ω1 (rad/s)',' ω2 (rad/s)',' ω3 (rad/s)'])
def get_static_pressure(self) -> float:
return self.fetch_value(' Pressure (Pa)')
def get_temperature(self) -> float:
raise NotImplementedError()
def get_longitude(self) -> float:
return self.fetch_value(' Longitude (°)')
def get_latitude(self) -> float:
return self.fetch_value(' Latitude (°)')
def get_altitude(self) -> float:
return self.fetch_value(' Z (m)')