Issue #144: Body-fixed point mapping

src/bsk_rl/data/rso_data.py

@@ -86,9 +86,9 @@ def get_log_state(self) -> list[list[bool]]:
 
         inspected_logs = []
         for recorder in self.point_access_recorders:
-            # inspected = np.logical_and(imaging_req, recorder.hasAccess)
-            inspected = recorder.hasAccess
-            inspected_logs.append(list(inspected))
+            inspected = np.logical_and(imaging_req, recorder.hasAccess)
+            # inspected = recorder.hasAccess
+            inspected_logs.append(list(np.array(inspected)))
 
         self.clear_recorders()
 
@@ -103,7 +103,6 @@ def compare_log_states(self, _, inspected_logs) -> Data:
             self.data.point_inspect_status.keys(), inspected_logs
         ):
             if any(log):
-                print(log)
                 point_inspect_status[rso_point] = True
 
         if len(point_inspect_status) > 0:

src/bsk_rl/gym.py

@@ -281,6 +281,12 @@ def reset(
             time_limit=self.time_limit,
         )
 
+        self.scenario.reset_during_sim_init()
+        self.rewarder.reset_during_sim_init()
+        self.communicator.reset_during_sim_init()
+
+        self.simulator.finish_init()
+
         self.scenario.reset_post_sim_init()
         self.rewarder.reset_post_sim_init()
         self.communicator.reset_post_sim_init()

src/bsk_rl/scene/rso_points.py

@@ -7,12 +7,10 @@
 
 import numpy as np
 import pandas as pd
-from Basilisk.utilities import orbitalMotion
 
 from bsk_rl.scene import Scenario
 from bsk_rl.sim.dyn import RSODynModel, RSOImagingDynModel
 from bsk_rl.sim.fsw import RSOImagingFSWModel
-from bsk_rl.utils.orbital import lla2ecef
 
 if TYPE_CHECKING:  # pragma: no cover
     from bsk_rl.data.base import Data
@@ -40,11 +38,9 @@ def reset_overwrite_previous(self) -> None:
 
     def reset_pre_sim_init(self) -> None:
         self.rso_points = self.generate_points()
-        return super().reset_pre_sim_init()
 
-    def reset_post_sim_init(self) -> None:
         # Check for RSOs and observers
-        rsos = [sat for sat in self.satellites if isinstance(sat.dynamics, RSODynModel)]
+        rsos = [sat for sat in self.satellites if issubclass(sat.dyn_type, RSODynModel)]
         if len(rsos) == 0:
             logger.warning("No RSODynModel satellites found in scenario.")
             return
@@ -54,12 +50,15 @@ def reset_post_sim_init(self) -> None:
         self.observers = [
             sat
             for sat in self.satellites
-            if isinstance(sat.dynamics, RSOImagingDynModel)
+            if issubclass(sat.dyn_type, RSOImagingDynModel)
         ]
         if len(self.observers) == 0:
             logger.warning("No RSOImagingDynModel satellites found in scenario.")
             return
 
+        return super().reset_pre_sim_init()
+
+    def reset_during_sim_init(self) -> None:
         # Add points to dynamics and fsw of RSO
         assert isinstance(self.rso.dynamics, RSODynModel)
         logger.debug("Adding inspection points to RSO and observers")

src/bsk_rl/sim/dyn.py

@@ -1252,22 +1252,32 @@ def __init__(self, *args, **kwargs) -> None:
 
     def _setup_dynamics_objects(self, **kwargs) -> None:
         super()._setup_dynamics_objects(**kwargs)
+
+        rso_dyn_proc_name = "RSODynProcess"
+        self.rso_dyn_proc = self.simulator.CreateNewProcess(rso_dyn_proc_name, 1)
+        self.rso_task_name = "RSODynTask"
+        self.rso_dyn_proc.addTask(
+            self.simulator.CreateNewTask(
+                self.rso_task_name, macros.sec2nano(self.dyn_rate)
+            )
+        )
+
         self.rso_points = []
 
     def add_rso_point(self, r_LB_B, aHat_B, theta, range):
         rso_point_model = spacecraftLocation.SpacecraftLocation()
         rso_point_model.primaryScStateInMsg.subscribeTo(self.scObject.scStateOutMsg)
-        rso_point_model.planetInMsg.subscribeTo(
-            self.world.gravFactory.spiceObject.planetStateOutMsgs[self.world.body_index]
-        )
-        rso_point_model.rEquator = self.simulator.world.planet.radEquator
-        rso_point_model.rPolar = self.simulator.world.planet.radEquator * 0.98
+        # rso_point_model.planetInMsg.subscribeTo(
+        #     self.world.gravFactory.spiceObject.planetStateOutMsgs[self.world.body_index]
+        # )
+        rso_point_model.rEquator = 1.0  # self.simulator.world.planet.radEquator
+        rso_point_model.rPolar = 1.0  # self.simulator.world.planet.radEquator * 0.98
         rso_point_model.r_LB_B = r_LB_B
         rso_point_model.aHat_B = aHat_B
         rso_point_model.theta = theta
         rso_point_model.maximumRange = range
         self.simulator.AddModelToTask(
-            self.task_name, rso_point_model, ModelPriority=2000
+            self.rso_task_name, rso_point_model, ModelPriority=1
         )
 
         self.rso_points.append(rso_point_model)

src/bsk_rl/sim/simulator.py

@@ -57,6 +57,8 @@ def __init__(
             self.dynamics_list[satellite.name] = satellite.set_dynamics(self.sim_rate)
             self.fsw_list[satellite.name] = satellite.set_fsw(self.sim_rate)
 
+    def finish_init(self) -> None:
+        """Finish simulator initialization."""
         self.InitializeSimulation()
         self.ConfigureStopTime(0)
         self.ExecuteSimulation()

src/bsk_rl/utils/functional.py

@@ -200,6 +200,10 @@ def reset_pre_sim_init(self) -> None:
         """Reset before simulator initialization."""
         pass
 
+    def reset_during_sim_init(self) -> None:
+        """Reset after simulator models have been created but before the simulator is initialized."""
+        pass
+
     def reset_post_sim_init(self) -> None:
         """Reset after simulator initialization."""
         pass