[#328] Attitude control action

Author: Mark2000

docs/source/release_notes.rst

@@ -36,6 +36,7 @@ Development - |version|
   By inheriting from a FSW class that overrides the ``MRPControlTask``, such as
   :class:`~bsk_rl.sim.fsw.BasicFSWModel` or :class:`~bsk_rl.sim.fsw.SteeringFSWModel`,
   users can implement custom attitude control strategies.
+* Add a continuous action for setting attitude in :class:`~bsk_rl.act.AttitudeSetpoint`.
 
 Version 1.2.0
 -------------

src/bsk_rl/act/__init__.py

@@ -54,13 +54,15 @@ class MyActionSatellite(Satellite):
 +-----------------------------+-------------+-------------------------------------------------------------------------------------------------------+
 | :class:`ImpulsiveThrustHill`| 4           | Like :class:`ImpulsiveThrust`, but specified in the Hill frame of another satellite.                  |
 +-----------------------------+-------------+-------------------------------------------------------------------------------------------------------+
-
+| :class:`AttitudeSetpoint`   | 3           | Set the inertial attitude setpoint using Modified Rodrigues Parameters (MRPs).                        |
++-----------------------------+-------------+-------------------------------------------------------------------------------------------------------+
 
 
 """
 
 from bsk_rl.act.actions import Action
 from bsk_rl.act.continuous_actions import (
+    AttitudeSetpoint,
     ContinuousAction,
     ImpulsiveThrust,
     ImpulsiveThrustHill,
@@ -85,6 +87,7 @@ class MyActionSatellite(Satellite):
     "DiscreteFSWAction",
     "Charge",
     "Drift",
+    "NadirPoint",
     "Desat",
     "Downlink",
     "Image",
@@ -93,4 +96,5 @@ class MyActionSatellite(Satellite):
     "ContinuousAction",
     "ImpulsiveThrust",
     "ImpulsiveThrustHill",
+    "AttitudeSetpoint",
 ]

src/bsk_rl/act/continuous_actions.py

@@ -183,3 +183,50 @@ def set_action(self, action: np.ndarray) -> None:
         dv_N = NH @ dv_H
 
         super().set_action(np.concatenate((dv_N, [dt])))
+
+
+class AttitudeSetpoint(ContinuousAction):
+    def __init__(
+        self,
+        name: str = "attitude_act",
+        control_period: float = 60,
+    ) -> None:
+        """Set the attitude command to the specified MRP.
+
+        Args:
+            name: Name of the action.
+            control_period: Control period for the action. [s]
+        """
+        super().__init__(name)
+        self.control_period = control_period
+
+    @property
+    def space(self) -> spaces.Box:
+        """Return the action space."""
+        return spaces.Box(
+            low=np.array([-1, -1, -1]),
+            high=np.array([1, 1, 1]),
+            shape=(3,),
+            dtype=np.float32,
+        )
+
+    @property
+    def action_description(self) -> list[str]:
+        """Description of the continuous action space."""
+        return ["mrp_1", "mrp_2", "mrp_3"]
+
+    def set_action(self, action: np.ndarray) -> None:
+        """Orient the satellite to the specified attitude.
+
+        Args:
+            action: vector of [mrp_1, mrp_2, mrp_3]
+        """
+        assert len(action) == 3, "Action must have 3 elements."
+
+        self.satellite.logger.info(
+            f"Setting attitude command to {action} for {self.control_period} seconds."
+        )
+        self.satellite.fsw.action_attitude_mrp(action)
+        self.satellite.update_timed_terminal_event(
+            self.satellite.simulator.sim_time + self.control_period
+        )

src/bsk_rl/sim/fsw/base.py

@@ -247,7 +247,11 @@ def _zero_gateway_msgs(self) -> None:
         self.attGuidMsg.write(messaging.AttGuidMsgPayload())
 
     def _make_task_list(self):
-        return [self.MRPControlTask(self), self.NadirPointTask(self)]
+        return [
+            self.MRPControlTask(self),
+            self.NadirPointTask(self),
+            self.TrackingErrorTask(self),
+        ]
 
     class MRPControlTask(Task):
         """Task to control the satellite attitude magically (i.e. without actuators)."""
@@ -315,6 +319,38 @@ def action_nadir_point(self) -> None:
             BasicFSWModel.NadirPointTask.name + self.satellite.name
         )
 
+    class TrackingErrorTask(Task):
+        """Task to convert an attitude reference to guidance."""
+
+        name = "trackingErrTask"
+
+        def __init__(self, fsw, priority=90) -> None:  # noqa: D107
+            """Task to convert an attitude reference to guidance."""
+            super().__init__(fsw, priority)
+
+        def _create_module_data(self) -> None:
+            self.trackingError = self.fsw.trackingError = (
+                attTrackingError.attTrackingError()
+            )
+            self.trackingError.ModelTag = "trackingError"
+
+        def _setup_fsw_objects(self, **kwargs) -> None:
+            self.trackingError.attNavInMsg.subscribeTo(
+                self.fsw.dynamics.simpleNavObject.attOutMsg
+            )
+            self.trackingError.attRefInMsg.subscribeTo(self.fsw.attRefMsg)
+            messaging.AttGuidMsg_C_addAuthor(
+                self.trackingError.attGuidOutMsg, self.fsw.attGuidMsg
+            )
+
+            self._add_model_to_task(self.trackingError, priority=1197)
+
+    @action
+    def action_attitude_mrp(self, mrp: np.ndarray) -> None:
+        """Point the satellite to the specified attitude."""
+        self.attRefMsg.write(messaging.AttRefMsgPayload(sigma_RN=mrp))
+        self.simulator.enableTask(self.TrackingErrorTask.name + self.satellite.name)
+
 
 class BasicFSWModel(FSWModel):
     """Basic FSW model with minimum necessary Basilisk components."""
@@ -327,7 +363,6 @@ def _make_task_list(self) -> list[Task]:
         return [
             self.SunPointTask(self),
             self.RWDesatTask(self),
-            self.TrackingErrorTask(self),
         ] + super()._make_task_list()
 
     def _set_config_msgs(self) -> None:
@@ -538,32 +573,6 @@ def action_desat(self) -> None:
             raise ValueError(f"{self.desatAttitude} not a valid desatAttitude")
         self.simulator.enableTask(self.TrackingErrorTask.name + self.satellite.name)
 
-    class TrackingErrorTask(Task):
-        """Task to convert an attitude reference to guidance."""
-
-        name = "trackingErrTask"
-
-        def __init__(self, fsw, priority=90) -> None:  # noqa: D107
-            """Task to convert an attitude reference to guidance."""
-            super().__init__(fsw, priority)
-
-        def _create_module_data(self) -> None:
-            self.trackingError = self.fsw.trackingError = (
-                attTrackingError.attTrackingError()
-            )
-            self.trackingError.ModelTag = "trackingError"
-
-        def _setup_fsw_objects(self, **kwargs) -> None:
-            self.trackingError.attNavInMsg.subscribeTo(
-                self.fsw.dynamics.simpleNavObject.attOutMsg
-            )
-            self.trackingError.attRefInMsg.subscribeTo(self.fsw.attRefMsg)
-            messaging.AttGuidMsg_C_addAuthor(
-                self.trackingError.attGuidOutMsg, self.fsw.attGuidMsg
-            )
-
-            self._add_model_to_task(self.trackingError, priority=1197)
-
     class MRPControlTask(Task):
         """Task to control the satellite attitude using reaction wheels."""
 

tests/integration/act/test_int_actions_continuous.py

@@ -1,5 +1,6 @@
 import gymnasium as gym
 import numpy as np
+import pytest
 from pytest import approx
 
 from bsk_rl import act, obs, sats
@@ -68,3 +69,29 @@ def test_thrust(self):
         self.env.step([0.0, 0.0, 0.0, 5.0])
         time_after = self.env.unwrapped.simulator.sim_time
         assert time_after - time_before == approx(5.0)
+
+
+class TestAttitudeSetpoint:
+    @pytest.mark.parametrize("basic_types", [True, False])
+    def test_attitude(self, basic_types):
+        class AttitudeSat(sats.Satellite):
+            if basic_types:
+                dyn_type = dyn.BasicDynamicsModel
+                fsw_type = fsw.BasicFSWModel
+            else:
+                dyn_type = dyn.DynamicsModel
+                fsw_type = fsw.FSWModel
+            observation_spec = [obs.SatProperties(dict(prop="sigma_BN"))]
+            action_spec = [act.AttitudeSetpoint(control_period=360.0)]
+
+        env = gym.make(
+            "SatelliteTasking-v1",
+            satellite=AttitudeSat("AttitudeSat", sat_args=dict()),
+            sim_rate=0.1,
+            time_limit=3600,
+            disable_env_checker=True,
+        )
+
+        env.reset()
+        observation, _, _, _, _ = env.step([0.1, 0.2, 0.3])
+        assert np.allclose(observation, np.array([0.1, 0.2, 0.3]), atol=1e-4)