|
4 | 4 | import warnings |
5 | 5 | from copy import deepcopy |
6 | 6 | from functools import cached_property |
7 | | - |
| 7 | +from ..motors.pointmassmotor import PointMassMotor |
| 8 | +from ..rocket import PointMassRocket |
8 | 9 | import numpy as np |
9 | 10 | import simplekml |
10 | 11 | from scipy.integrate import BDF, DOP853, LSODA, RK23, RK45, OdeSolver, Radau |
@@ -1193,16 +1194,33 @@ def __init_solver_monitors(self): |
1193 | 1194 |
|
1194 | 1195 | def __init_equations_of_motion(self): |
1195 | 1196 | """Initialize equations of motion.""" |
1196 | | - if self.equations_of_motion == "solid_propulsion" and self.simulation_mode == '6 DOF': |
1197 | | - # NOTE: The u_dot is faster, but only works for solid propulsion |
1198 | | - self.u_dot_generalized = self.u_dot |
1199 | | - elif self.equations_of_motion == "solid_propulsion" and self.simulation_mode == '3 DOF': |
1200 | | - # NOTE: The u_dot is faster, but only works for solid propulsion |
1201 | | - self.u_dot_generalized = self.u_dot_3dof |
1202 | | - elif self.simulation_mode == '3 DOF': |
1203 | | - # NOTE: The u_dot is faster, but only works for solid propulsion |
1204 | | - self.u_dot_generalized = self.u_dot_generalized_3dof |
| 1197 | + # Determine if a point-mass model is used. |
| 1198 | + is_point_mass = ( |
| 1199 | + isinstance(self.rocket, PointMassRocket) |
| 1200 | + or (self.rocket._motors and isinstance(self.rocket._motors[0], PointMassMotor)) |
| 1201 | + ) |
| 1202 | + # Set simulation mode based on model type. |
| 1203 | + if is_point_mass: |
| 1204 | + if self.simulation_mode != "3 DOF": |
| 1205 | + warnings.warn("A point-mass model was detected. Simulation mode should be '3 DOF'.", UserWarning) |
| 1206 | + self.simulation_mode = "3 DOF" |
| 1207 | + else: |
| 1208 | + self.simulation_mode = self.simulation_mode |
1205 | 1209 |
|
| 1210 | + # Set the equations of motion based on the final simulation mode. |
| 1211 | + if self.simulation_mode == "3 DOF": |
| 1212 | + self.equations_of_motion = self.equations_of_motion |
| 1213 | + self.u_dot_generalized = self.u_dot_generalized_3dof |
| 1214 | + elif self.simulation_mode == "6 DOF": |
| 1215 | + if self.equations_of_motion == "solid_propulsion": |
| 1216 | + self.u_dot_generalized = self.u_dot_6dof |
| 1217 | + else: |
| 1218 | + self.u_dot_generalized = self.u_dot_generalized_6dof |
| 1219 | + else: |
| 1220 | + raise ValueError( |
| 1221 | + f"Invalid simulation_mode: {self.simulation_mode}. " |
| 1222 | + "Must be '3 DOF' or '6 DOF'." |
| 1223 | + ) |
1206 | 1224 | def __init_controllers(self): |
1207 | 1225 | """Initialize controllers and sensors""" |
1208 | 1226 | self._controllers = self.rocket._controllers[:] |
|
0 commit comments