mirror of
https://git.intern.spaceteamaachen.de/ALPAKA/SPATZ.git
synced 2025-06-10 18:15:59 +00:00
Major rework using rocketpy
This commit is contained in:
parent
1b06db4152
commit
382cb9aad4
3
.vscode/launch.json
vendored
3
.vscode/launch.json
vendored
@ -9,7 +9,8 @@
|
|||||||
"type": "debugpy",
|
"type": "debugpy",
|
||||||
"request": "launch",
|
"request": "launch",
|
||||||
"program": "testing.py",
|
"program": "testing.py",
|
||||||
"console": "integratedTerminal"
|
"console": "integratedTerminal",
|
||||||
|
"justMyCode": false
|
||||||
}
|
}
|
||||||
]
|
]
|
||||||
}
|
}
|
5001
dummy2.csv
Normal file
5001
dummy2.csv
Normal file
File diff suppressed because it is too large
Load Diff
2646
nominal_wind.csv
Normal file
2646
nominal_wind.csv
Normal file
File diff suppressed because it is too large
Load Diff
36
rework_test.py
Normal file
36
rework_test.py
Normal 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')
|
@ -4,43 +4,7 @@ import pandas as pd
|
|||||||
from typing import Any, Tuple, List
|
from typing import Any, Tuple, List
|
||||||
from numpy.typing import ArrayLike
|
from numpy.typing import ArrayLike
|
||||||
|
|
||||||
from abc import abstractmethod
|
from spatz.simulations.advanceable import Advanceable
|
||||||
|
|
||||||
|
|
||||||
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
|
|
||||||
|
|
||||||
|
|
||||||
class Logger(Advanceable):
|
class Logger(Advanceable):
|
||||||
|
@ -1,6 +1,6 @@
|
|||||||
from spatz.sensors.sensor import Sensor
|
from spatz.sensors.sensor import Sensor
|
||||||
from spatz.sensors.gps import GPS
|
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.pressure import PressureSensor
|
||||||
from spatz.sensors.temperature import TemperatureSensor
|
from spatz.sensors.temperature import TemperatureSensor
|
||||||
from spatz.sensors.compound import CompoundSensor
|
from spatz.sensors.compound import CompoundSensor
|
@ -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.gyroscope import Gyroscope
|
||||||
from spatz.sensors.imu.imu import IMU
|
from spatz.sensors.imu.imu import IMU
|
@ -1,11 +1,13 @@
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
from typing import List
|
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.sensors import Sensor
|
||||||
from spatz.transforms import Transform
|
from spatz.transforms import Transform
|
||||||
from spatz.dataset import Dataset
|
from spatz.dataset import Dataset
|
||||||
|
from spatz.simulations.data_source import DataSource
|
||||||
from spatz.logger import Logger
|
from spatz.logger import Logger
|
||||||
|
|
||||||
|
|
||||||
@ -18,33 +20,61 @@ __all__=[
|
|||||||
g = 9.81
|
g = 9.81
|
||||||
|
|
||||||
|
|
||||||
|
class CoordSystem:
|
||||||
|
LEFT_HANDED = 0,
|
||||||
|
RIGHT_HANDED = 0,
|
||||||
|
|
||||||
|
|
||||||
class Accelerometer(Sensor):
|
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)
|
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._offset = np.array([offset, 0, 0])
|
||||||
|
self._coord_system = coord_system
|
||||||
|
self._orientation = orientation
|
||||||
|
|
||||||
def _get_data(self) -> ArrayLike | float:
|
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('global_ax', acc[0], self._get_name())
|
||||||
self._logger.write('FL_y', acc[1], self._get_name())
|
# self._logger.write('global_ay', acc[1], self._get_name())
|
||||||
self._logger.write('FL_z', acc[2], self._get_name())
|
# self._logger.write('global_az', acc[2], self._get_name())
|
||||||
|
|
||||||
# Convert FL to body
|
# Convert FL to body
|
||||||
mat = self._dataset.launch_rail_to_body()
|
acc = self._dataset.global_to_local() @ acc
|
||||||
acc = mat @ acc
|
# acc += mat @ np.array([0, 0, g])
|
||||||
acc += mat @ np.array([0, 0, g])
|
|
||||||
|
|
||||||
self._logger.write('B_x', acc[0], self._get_name())
|
# self._logger.write('local_x', acc[0], self._get_name())
|
||||||
self._logger.write('B_y', acc[1], self._get_name())
|
# self._logger.write('local_y', acc[1], self._get_name())
|
||||||
self._logger.write('B_z', acc[2], self._get_name())
|
# self._logger.write('local_z', acc[2], self._get_name())
|
||||||
|
|
||||||
# Flip axes to sensor's perspective.
|
# Flip axes to sensor's perspective. Spatz uses a right-handed coordinate system.
|
||||||
acc *= -1
|
#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.
|
# 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)))
|
acc += (np.cross(omega, self._offset) + np.cross(omega, np.cross(omega, self._offset)))
|
||||||
|
|
||||||
return acc
|
return acc
|
||||||
|
46
spatz/sensors/imu/bmi088.py
Normal file
46
spatz/sensors/imu/bmi088.py
Normal 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)
|
@ -4,17 +4,18 @@ from typing import List
|
|||||||
from spatz.sensors import Sensor
|
from spatz.sensors import Sensor
|
||||||
from spatz.transforms import Transform
|
from spatz.transforms import Transform
|
||||||
from spatz.dataset import Dataset
|
from spatz.dataset import Dataset
|
||||||
|
from spatz.simulations.data_source import DataSource
|
||||||
from spatz.logger import Logger
|
from spatz.logger import Logger
|
||||||
|
|
||||||
|
|
||||||
class Gyroscope(Sensor):
|
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)
|
super().__init__(dataset, logger, transforms)
|
||||||
|
|
||||||
self._offset = offset
|
self._offset = offset
|
||||||
|
|
||||||
def _get_data(self) -> ArrayLike | float:
|
def _get_data(self) -> ArrayLike | float:
|
||||||
# Rotation in rad/sec
|
# Rotation in rad/sec
|
||||||
x = self._dataset.get_angular_velocities()
|
x = self._dataset.get_angular_velocity()
|
||||||
|
|
||||||
return x
|
return x
|
||||||
|
30
spatz/sensors/imu/h3lis100dl.py
Normal file
30
spatz/sensors/imu/h3lis100dl.py
Normal 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
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -3,8 +3,9 @@ import numpy as np
|
|||||||
from typing import AnyStr, List
|
from typing import AnyStr, List
|
||||||
from numpy.typing import ArrayLike
|
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.transforms import Transform, GaussianNoise
|
||||||
|
from spatz.simulations.data_source import DataSource
|
||||||
from spatz.dataset import Dataset
|
from spatz.dataset import Dataset
|
||||||
from spatz.logger import Logger
|
from spatz.logger import Logger
|
||||||
|
|
||||||
@ -14,8 +15,8 @@ class WSEN_ISDS(IMU):
|
|||||||
|
|
||||||
|
|
||||||
class WSEN_ISDS_ACC(Accelerometer):
|
class WSEN_ISDS_ACC(Accelerometer):
|
||||||
def __init__(self, dataset: Dataset, logger: Logger, offset: float, transforms: List[Transform] = []):
|
def __init__(self, dataset: DataSource, logger: Logger, orientation=np.identity(3), offset=0, transforms: List[Transform] = []):
|
||||||
super().__init__(dataset, logger, offset, transforms)
|
super().__init__(dataset, logger, CoordSystem.LEFT_HANDED, orientation, offset, transforms)
|
||||||
|
|
||||||
self.__variance = 0.05
|
self.__variance = 0.05
|
||||||
self.__noise = GaussianNoise(np.zeros(3), np.identity(3) * self.__variance)
|
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_y_noise', noise[1], self._get_name())
|
||||||
self._logger.write('acc_z_noise', noise[2], 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
|
return y
|
||||||
|
|
||||||
|
|
||||||
class WSEN_ISDS_GYRO(Gyroscope):
|
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)
|
super().__init__(dataset, logger, offset, transforms)
|
||||||
|
|
||||||
def _get_name(self) -> AnyStr:
|
def _get_name(self) -> AnyStr:
|
||||||
@ -51,4 +56,8 @@ class WSEN_ISDS_GYRO(Gyroscope):
|
|||||||
|
|
||||||
# TODO: Noise model.
|
# TODO: Noise model.
|
||||||
|
|
||||||
|
self._log('ox', x[0])
|
||||||
|
self._log('oy', x[1])
|
||||||
|
self._log('oz', x[2])
|
||||||
|
|
||||||
return x
|
return x
|
||||||
|
@ -3,12 +3,13 @@ from numpy.typing import ArrayLike
|
|||||||
|
|
||||||
from spatz.sensors import PressureSensor
|
from spatz.sensors import PressureSensor
|
||||||
from spatz.dataset import Dataset, Phase
|
from spatz.dataset import Dataset, Phase
|
||||||
|
from spatz.simulations.data_source import DataSource
|
||||||
from spatz.logger import Logger
|
from spatz.logger import Logger
|
||||||
from spatz.transforms import GaussianNoise, Transform, ProportionalGaussian
|
from spatz.transforms import GaussianNoise, Transform, ProportionalGaussian
|
||||||
|
|
||||||
|
|
||||||
class MS5611(PressureSensor):
|
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)
|
super().__init__(dataset, logger, transforms, ts_effects)
|
||||||
|
|
||||||
# Noise model obtained by a test flight using this sensor.
|
# Noise model obtained by a test flight using this sensor.
|
||||||
|
@ -6,11 +6,12 @@ from typing import List
|
|||||||
from spatz.sensors import Sensor
|
from spatz.sensors import Sensor
|
||||||
from spatz.logger import Logger
|
from spatz.logger import Logger
|
||||||
from spatz.dataset import Dataset
|
from spatz.dataset import Dataset
|
||||||
|
from spatz.simulations.data_source import DataSource
|
||||||
from spatz.transforms import Transform
|
from spatz.transforms import Transform
|
||||||
|
|
||||||
|
|
||||||
class PressureSensor(Sensor):
|
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_
|
"""_summary_
|
||||||
|
|
||||||
Args:
|
Args:
|
||||||
@ -28,7 +29,7 @@ class PressureSensor(Sensor):
|
|||||||
self._ts_effects = active
|
self._ts_effects = active
|
||||||
|
|
||||||
def _get_data(self) -> float:
|
def _get_data(self) -> float:
|
||||||
x = self._dataset.get_pressure()
|
x = self._dataset.get_static_pressure()
|
||||||
|
|
||||||
if self._ts_effects:
|
if self._ts_effects:
|
||||||
# Pre-defined constants.
|
# Pre-defined constants.
|
||||||
@ -36,16 +37,16 @@ class PressureSensor(Sensor):
|
|||||||
sigma = 40
|
sigma = 40
|
||||||
|
|
||||||
# How far away from transsonic speed (mach 1) are we?
|
# 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())
|
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.
|
# 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))
|
ts_eff = _p * math.exp(-0.5* (dv / sigma)**2 ) / (sigma * math.sqrt(2*math.pi))
|
||||||
|
|
||||||
# Log the values for the transsonic effect.
|
# Log the values for the transsonic effect.
|
||||||
self._logger.write('ts_effects', ts_eff, domain=self._get_name())
|
self._log('ts_effects', ts_eff)
|
||||||
self._logger.write('mach_no', self._dataset.get_mach_number(), domain='mach')
|
self._log('mach_no', self._dataset.get_mach_number())
|
||||||
self._logger.write('speedofsound', self._dataset.get_speed_of_sound(), domain='mach')
|
self._log('speedofsound', self._dataset.get_speed_of_sound())
|
||||||
|
|
||||||
x = x + ts_eff
|
x = x + ts_eff
|
||||||
|
|
||||||
|
@ -9,17 +9,24 @@ from pandas import NA
|
|||||||
from spatz.transforms import *
|
from spatz.transforms import *
|
||||||
from spatz.logger import *
|
from spatz.logger import *
|
||||||
from spatz.dataset import *
|
from spatz.dataset import *
|
||||||
|
from spatz.simulations.data_source import DataSource
|
||||||
|
|
||||||
|
|
||||||
class Sensor:
|
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._dataset = dataset
|
||||||
self._logger = logger
|
self._logger = logger
|
||||||
self._transforms = transforms
|
self._transforms = transforms
|
||||||
self._min_value = min_value
|
self._min_value = min_value
|
||||||
self._max_value = max_value
|
self._max_value = max_value
|
||||||
|
|
||||||
def set_dataset(self, dataset: Dataset):
|
def set_dataset(self, dataset: DataSource):
|
||||||
self._dataset = dataset
|
self._dataset = dataset
|
||||||
|
|
||||||
def set_logger(self, logger: Logger):
|
def set_logger(self, logger: Logger):
|
||||||
|
@ -1 +1 @@
|
|||||||
from temperature import TemperatureSensor
|
from spatz.sensors.temperature.temperature import TemperatureSensor
|
@ -8,7 +8,7 @@ from spatz.transforms import Transform
|
|||||||
|
|
||||||
|
|
||||||
class MS5611Temperature(TemperatureSensor):
|
class MS5611Temperature(TemperatureSensor):
|
||||||
def __init__(self, dataset: Dataset, transforms: List[Transform] = ...):
|
def __init__(self, dataset: Dataset, transforms: List[Transform] = []):
|
||||||
super().__init__(dataset, transforms)
|
super().__init__(dataset, transforms)
|
||||||
|
|
||||||
self.__noise = GaussianNoise(0, 0.5)
|
self.__noise = GaussianNoise(0, 0.5)
|
||||||
|
@ -2,6 +2,8 @@ from typing import List
|
|||||||
from numpy.random import normal
|
from numpy.random import normal
|
||||||
from tqdm import tqdm
|
from tqdm import tqdm
|
||||||
|
|
||||||
|
from spatz.simulations.advanceable import Advanceable
|
||||||
|
from spatz.simulations.data_source import DataSource
|
||||||
from spatz.dataset import Dataset
|
from spatz.dataset import Dataset
|
||||||
from spatz.logger import Logger
|
from spatz.logger import Logger
|
||||||
from spatz.sensors import Sensor
|
from spatz.sensors import Sensor
|
||||||
@ -35,9 +37,11 @@ class UniformTimeSteps:
|
|||||||
return self.__dt + noise
|
return self.__dt + noise
|
||||||
|
|
||||||
|
|
||||||
class Simulation:
|
class Simulation(Advanceable):
|
||||||
def __init__(self, time_steps=UniformTimeSteps(0.01)):
|
def __init__(self, time_steps=UniformTimeSteps(0.01)):
|
||||||
self.__dataset = None
|
super().__init__()
|
||||||
|
|
||||||
|
self.__data_source = None
|
||||||
self.__logger = None
|
self.__logger = None
|
||||||
self.__sensors: List[Sensor] = []
|
self.__sensors: List[Sensor] = []
|
||||||
self.__time_steps = time_steps
|
self.__time_steps = time_steps
|
||||||
@ -46,26 +50,25 @@ class Simulation:
|
|||||||
idx = 0
|
idx = 0
|
||||||
|
|
||||||
# Clear all logs and reset the dataset to the first time step.
|
# Clear all logs and reset the dataset to the first time step.
|
||||||
self.__dataset.reset()
|
self.__data_source.reset()
|
||||||
self.__logger.reset()
|
self.__logger.reset()
|
||||||
|
|
||||||
if verbose:
|
if verbose:
|
||||||
pbar = tqdm(total=self.__dataset.get_length())
|
pbar = tqdm(total=self.__data_source.get_length())
|
||||||
|
|
||||||
while True:
|
while True:
|
||||||
t = self.__dataset.get_time()
|
t = self.get_time()
|
||||||
dt = self.__time_steps(t)
|
dt = self.__time_steps(t)
|
||||||
t_ = t + dt
|
t_ = t + dt
|
||||||
idx += 1
|
idx += 1
|
||||||
|
|
||||||
if t_ > self.__dataset.get_length():
|
if t_ > self.__data_source.get_length():
|
||||||
break
|
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
|
break
|
||||||
|
|
||||||
self.__dataset.step(dt)
|
self.advance(dt)
|
||||||
self.__logger.step(dt)
|
|
||||||
|
|
||||||
if verbose:
|
if verbose:
|
||||||
pbar.update(dt)
|
pbar.update(dt)
|
||||||
@ -75,31 +78,26 @@ class Simulation:
|
|||||||
if verbose:
|
if verbose:
|
||||||
pbar.close()
|
pbar.close()
|
||||||
|
|
||||||
def advance_to(self, t: float):
|
def _on_step(self, dt: float):
|
||||||
t_old = self.__dataset.get_time()
|
self.__data_source.advance(dt)
|
||||||
self.__dataset.step(t - t_old)
|
self.__logger.advance(dt)
|
||||||
self.__logger.step(t - t_old)
|
|
||||||
|
|
||||||
def advance(self, dt: float):
|
def load(self, source: DataSource):
|
||||||
self.__dataset.step(dt)
|
self.__data_source = source
|
||||||
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)
|
|
||||||
self.__logger = Logger()
|
self.__logger = Logger()
|
||||||
|
|
||||||
for sensor in self.__sensors:
|
for sensor in self.__sensors:
|
||||||
sensor.set_dataset(self.__dataset)
|
sensor.set_dataset(self.__data_source)
|
||||||
sensor.set_logger(self.__logger)
|
sensor.set_logger(self.__logger)
|
||||||
|
|
||||||
return self
|
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:
|
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
|
"""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.
|
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."
|
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]
|
return self.__sensors[-1]
|
||||||
|
|
||||||
@ -132,10 +130,10 @@ class Simulation:
|
|||||||
attributes = observer_or_attributes
|
attributes = observer_or_attributes
|
||||||
assert len(attributes) != 0, "Observed attributes list must be nonempty."
|
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:
|
else:
|
||||||
observer = observer_or_attributes
|
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]
|
return self.__sensors[-1]
|
||||||
|
|
||||||
|
47
spatz/simulations/advanceable.py
Normal file
47
spatz/simulations/advanceable.py
Normal 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
|
91
spatz/simulations/csv_source.py
Normal file
91
spatz/simulations/csv_source.py
Normal 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])
|
74
spatz/simulations/data_source.py
Normal file
74
spatz/simulations/data_source.py
Normal 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
|
87
spatz/simulations/rocketpy.py
Normal file
87
spatz/simulations/rocketpy.py
Normal 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)')
|
||||||
|
|
||||||
|
|
Loading…
x
Reference in New Issue
Block a user