diff --git a/src/bsk_rl/sim/dyn.py b/src/bsk_rl/sim/dyn.py index 479027e..14c3477 100644 --- a/src/bsk_rl/sim/dyn.py +++ b/src/bsk_rl/sim/dyn.py @@ -70,6 +70,7 @@ aliveness_checker, check_aliveness_checkers, default_args, + valid_func_name, ) from bsk_rl.utils.orbital import random_orbit, rv2HN, rv2omega @@ -1185,6 +1186,60 @@ def __init__(self, *args, **kwargs) -> None: super().__init__(*args, **kwargs) +class ConjunctionDynModel(BasicDynamicsModel): + """For evaluating conjunctions between satellites.""" + + def __init__(self, *args, **kwargs) -> None: + """For evaluating conjunctions between satellites.""" + super().__init__(*args, **kwargs) + self.conjunctions = [] + + def _setup_dynamics_objects(self, **kwargs) -> None: + super()._setup_dynamics_objects(**kwargs) + self.setup_conjunctions(**kwargs) + + @aliveness_checker + def conjunction_valid(self) -> bool: + """Check if conjunction has not occured.""" + return len(self.conjunctions) == 0 + + @default_args(conjunction_radius=10) + def setup_conjunctions(self, conjunction_radius: float, **kwargs) -> None: + """Set up conjunction checking between satellites. + + Args: + conjunction_radius: [m] Minimum distance for a conjunction. + kwargs: Passed to other setup functions. + """ + self.conjunction_radius = conjunction_radius + + for sat_dyn in self.simulator.dynamics_list.values(): + if sat_dyn != self and isinstance(sat_dyn, ConjunctionDynModel): + self.simulator.createNewEvent( + valid_func_name( + f"conjunction_{self.satellite.name}_{sat_dyn.satellite.name}" + ), + macros.sec2nano(self.simulator.sim_rate), + True, + [ + f"np.linalg.norm(np.array({self.satellite._satellite_command}.dynamics.r_BN_N) - np.array({sat_dyn.satellite._satellite_command}.dynamics.r_BN_N))" + + " <= " + + f"{self.satellite._satellite_command}.dynamics.conjunction_radius + {sat_dyn.satellite._satellite_command}.dynamics.conjunction_radius" + ], + [ + self.satellite._info_command( + f"collided with {sat_dyn.satellite.name}" + ), + sat_dyn.satellite._info_command( + f"collided with {self.satellite.name}" + ), + f"{self.satellite._satellite_command}.dynamics.conjunctions.append({sat_dyn.satellite._satellite_command})", + f"{sat_dyn.satellite._satellite_command}.dynamics.conjunctions.append({self.satellite._satellite_command})", + ], + terminal=True, + ) + + __doc_title__ = "Dynamics Sims" __all__ = [ "DynamicsModel", @@ -1193,5 +1248,6 @@ def __init__(self, *args, **kwargs) -> None: "ImagingDynModel", "ContinuousImagingDynModel", "GroundStationDynModel", + "ConjunctionDynModel", "FullFeaturedDynModel", ]