[#266] Orbital element observations

Author: Mark2000

docs/source/release_notes.rst

@@ -31,6 +31,7 @@ Development - |version|
 * Allow for the ``transmitterPacketSize`` to be specified. The default sets it to the instrument's baud rate.
 * Add a maximum range checking dynamics model in :class:`~bsk_rl.dynamics.MaxRangeDynModel`. Useful for keeping an agent
   in the vicinity of a target early in training.
+* Add properties in spacecraft dynamics for orbital element observations.
 
 
 Version 1.1.0

src/bsk_rl/sim/dyn/base.py

@@ -211,6 +211,69 @@ def omega_BH_H(self):
         HN = rv2HN(self.r_BN_N, self.v_BN_N)
         return HN @ omega_BH_N
 
+    def _compute_oes(self):
+        if (
+            not hasattr(self, "_oe_cache_time")
+            or self._oe_cache_time != self.simulator.time
+        ):
+            self._oe_cache = orbitalMotion.rv2elem(
+                mu=self.mu, rVec=np.array(self.r_BN_N), vVec=np.array(self.v_BN_N)
+            )
+            self._oe_cache_time = getattr(self.simulator, "time", None)
+        return self._oe_cache
+
+    @property
+    def semi_major_axis(self):
+        """Semimajor axis of the satellite's orbit [km]."""
+        return self._compute_oes().a
+
+    @property
+    def eccentricity(self):
+        """Eccentricity of the satellite's orbit [-]."""
+        return self._compute_oes().e
+
+    @property
+    def inclination(self):
+        """Inclination of the satellite's orbit [rad]."""
+        return self._compute_oes().i
+
+    @property
+    def ascending_node(self):
+        """Longitude of ascending node of the satellite's orbit [rad]."""
+        return self._compute_oes().AN
+
+    @property
+    def argument_of_periapsis(self):
+        """Argument of periapsis of the satellite's orbit [rad]."""
+        return self._compute_oes().AP
+
+    @property
+    def true_anomaly(self):
+        """True anomaly of the satellite's orbit [rad]."""
+        return self._compute_oes().f
+
+    @property
+    def beta_angle(self):
+        """Beta angle of the satellite's orbit, between 0 and 2pi [rad].
+
+        The angle between the angular momentum vector and the sun direction vector.
+        """
+        r_BN_N = self.dynamics.r_BN_N
+        v_BN_N = self.dynamics.v_BN_N
+        h_N = np.cross(r_BN_N, v_BN_N)
+        r_SN_N = (
+            self.simulator.world.gravFactory.spiceObject.planetStateOutMsgs[
+                self.simulator.world.sun_index
+            ]
+            .read()
+            .PositionVector
+        )
+
+        beta = np.arccos(
+            np.dot(h_N, r_SN_N) / (np.linalg.norm(h_N) * np.linalg.norm(r_SN_N))
+        )
+        return beta
+
     @property
     def battery_charge(self):
         """Battery charge [W*s]."""
@@ -293,6 +356,7 @@ def setup_spacecraft_hub(
             priority: Model priority.
             kwargs: Passed to other setup functions.
         """
+        self.mu = mu
         if rN is not None and vN is not None and oe is None:
             pass
         elif oe is not None and rN is None and vN is None:

tests/integration/obs/test_int_observations.py

@@ -62,6 +62,7 @@ class SatPropertiesSat(sats.Satellite):
             obs.SatProperties(
                 dict(prop="r_BN_N", module="dynamics"),
                 dict(prop="r_BN_N", norm=7000.0 * 1e3),
+                dict(prop="inclination", module="dynamics", norm=np.pi / 180),
             ),
         ]
         action_spec = [act.Drift()]
@@ -71,7 +72,7 @@ class SatPropertiesSat(sats.Satellite):
         satellite=SatPropertiesSat(
             "Sputnik",
             sat_args=SatPropertiesSat.default_sat_args(
-                oe=random_orbit(r_body=7000, alt=0)
+                oe=random_orbit(r_body=7000, alt=0, i=45)
             ),
         ),
         scenario=UniformTargets(n_targets=0),
@@ -86,6 +87,7 @@ def test_normd_property_state(self):
         observation, info = self.env.reset()
         assert np.linalg.norm(observation[0:3]) == approx(7000.0 * 1e3)
         assert np.linalg.norm(observation[3:6]) == approx(1.0)
+        assert observation[6] == approx(45)  # Inclination in radians
 
 
 class TestTime:

tests/unittest/sim/test_dynamics.py

@@ -13,6 +13,7 @@
     ImagingDynModel,
     LOSCommDynModel,
 )
+from bsk_rl.sim.dyn import base as dyn_module
 
 module = "bsk_rl.sim.dyn."
 
@@ -79,6 +80,56 @@ def test_dynamic_properties(self):
         assert (dyn.v_BN_P == np.zeros(3)).all()
         assert (dyn.omega_BP_P == np.zeros(3)).all()
 
+    def test_orbit_properties(self):
+        with patch.object(dyn_module, "orbitalMotion") as mocked:
+            mocked.rv2elem = MagicMock(
+                return_value=MagicMock(a=1.0, e=2.0, i=3.0, AN=4.0, AP=5.0, f=6.0)
+            )
+            dyn = BasicDynamicsModel(MagicMock(simulator=MagicMock()), 1.0)
+            dyn.mu = 1
+            dyn.scObject = MagicMock()
+            dyn.scObject.scStateOutMsg.read.return_value = MagicMock(
+                r_BN_N=[1, 2, 3], v_BN_N=[4, 5, 6]
+            )
+            dyn.simulator.time = 0.0
+
+            # Check that call to rv2elem happens only once
+            assert dyn.semi_major_axis == 1.0
+            mocked.rv2elem.assert_called_once()
+            assert dyn.eccentricity == 2.0
+            assert dyn.inclination == 3.0
+            assert dyn.ascending_node == 4.0
+            assert dyn.argument_of_periapsis == 5.0
+            assert dyn.true_anomaly == 6.0
+            mocked.rv2elem.assert_called_once()
+
+    def test_orbit_properties_no_cache_update(self):
+        with patch.object(dyn_module, "orbitalMotion") as mocked:
+            mocked.rv2elem = MagicMock(return_value=MagicMock(a=1.0))
+            dyn = BasicDynamicsModel(MagicMock(simulator=MagicMock()), 1.0)
+            dyn.mu = 1
+            dyn.scObject = MagicMock()
+            dyn.scObject.scStateOutMsg.read.return_value = MagicMock()
+            dyn.simulator.time = 0.0
+
+            assert dyn.semi_major_axis == 1.0
+            mocked.rv2elem = MagicMock(return_value=MagicMock(a=2.0))
+            assert dyn.semi_major_axis == 1.0
+
+    def test_orbit_properties_cache_update(self):
+        with patch.object(dyn_module, "orbitalMotion") as mocked:
+            mocked.rv2elem = MagicMock(return_value=MagicMock(a=1.0))
+            dyn = BasicDynamicsModel(MagicMock(simulator=MagicMock()), 1.0)
+            dyn.mu = 1
+            dyn.scObject = MagicMock()
+            dyn.scObject.scStateOutMsg.read.return_value = MagicMock()
+            dyn.simulator.time = 0.0
+
+            assert dyn.semi_major_axis == 1.0
+            dyn.simulator.time = 1.0
+            mocked.rv2elem = MagicMock(return_value=MagicMock(a=2.0))
+            assert dyn.semi_major_axis == 2.0
+
     def test_battery_properties(self):
         dyn = BasicDynamicsModel(MagicMock(simulator=MagicMock()), 1.0)
         dyn.powerMonitor = MagicMock()