Merge pull request #488 from AVSLab/feature/487-thrusterPlatformState

Feature/487 thruster platform state

docs/source/Support/bskReleaseNotes.rst

@@ -73,6 +73,7 @@ Version |release|
   error. Rather, the process return code (0 for success, and anything else for failure) indicates the success.
 - The ``MAX_N_CSS_MEAS`` define is increased to 32 matching the maximum number of coarse sun sensors.
 - mixed bug in time to nano-seconds conversions in ``macros.py`` support file
+- Created :ref:`thrusterPlatformState` to map the thruster configuration information to body frame given the time-varying platform states.
 
 
 Version 2.2.0 (June 28, 2023)

src/fswAlgorithms/effectorInterfaces/thrusterPlatformState/_UnitTest/test_thrusterPlatformState.py

@@ -0,0 +1,168 @@
+#
+#  ISC License
+#
+#  Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
+#
+#  Permission to use, copy, modify, and/or distribute this software for any
+#  purpose with or without fee is hereby granted, provided that the above
+#  copyright notice and this permission notice appear in all copies.
+#
+#  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+#  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+#  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+#  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+#  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+#  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+#  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+#
+
+
+import pytest
+import os, inspect, random
+import numpy as np
+
+filename = inspect.getframeinfo(inspect.currentframe()).filename
+path = os.path.dirname(os.path.abspath(filename))
+bskName = 'Basilisk'
+splitPath = path.split(bskName)
+
+
+# Import all the modules that are going to be called in this simulation
+from Basilisk.utilities import SimulationBaseClass
+from Basilisk.fswAlgorithms import thrusterPlatformState
+from Basilisk.utilities import macros
+from Basilisk.utilities import RigidBodyKinematics as rbk
+from Basilisk.architecture import messaging
+from Basilisk.architecture import bskLogging
+
+
+@pytest.mark.parametrize("theta1", [0, np.pi/36, np.pi/18])
+@pytest.mark.parametrize("theta2", [0, np.pi/36, np.pi/18])
+@pytest.mark.parametrize("accuracy", [1e-10])
+# update "module" in this function name to reflect the module name
+def test_platformRotation(show_plots, theta1, theta2, accuracy):
+    r"""
+    **Validation Test Description**
+
+    This unit test script tests the correctness of the output thruster configuration msg outputted by
+    :ref:`thrusterPlatformState`. The correctness of the output is determined checking that the thrust unit direction
+    vector, magnitude, and application point, match the rigid body rotation described by the input tip and tild angles
+    theta1 and theta2.
+
+    **Test Parameters**
+
+    This test provides input tip and tilt angles to the module, as well as the thruster configuration information
+    expressed with respect to the platform frame F.
+
+    Args:
+        theta1 (rad): platform tip angle
+        theta2 (rad): platform tilt angle
+        accuracy (float): accuracy within which results are considered to match the truth values.
+
+    **Description of Variables Being Tested**
+
+    In this test, offsets are given between the thrust application point and the origin of the platform frame
+    (:math:`r_{T/F}`), and between the origin of the platform frame and the origin of the mount frame (:math:`r_{F/M}`).
+    These offset vectors are hard coded into the unit test. The test checks the correctness of the output thrust unit
+    direction vector and magnitude in the body frame, as well as the thrust application point location with respect to
+    the origin of the body frame B, in body frame coordinates.
+    """
+    # each test method requires a single assert method to be called
+    platformRotationTestFunction(show_plots, theta1, theta2, accuracy)
+
+
+def platformRotationTestFunction(show_plots, theta1, theta2, accuracy):
+
+    sigma_MB = np.array([0., 0., 0.])
+    r_BM_M = np.array([0.0, 0.1, 1.4])
+    r_FM_F = np.array([0.0, 0.0, -0.1])
+    r_TF_F = np.array([-0.01, 0.03, 0.02])
+    T_F    = np.array([1.0, 1.0, 10.0])
+
+    unitTaskName = "unitTask"                # arbitrary name (don't change)
+    unitProcessName = "TestProcess"          # arbitrary name (don't change)
+    bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING)
+
+    # Create a sim module as an empty container
+    unitTestSim = SimulationBaseClass.SimBaseClass()
+
+    # Create test thread
+    testProcessRate = macros.sec2nano(1)     # update process rate update time
+    testProc = unitTestSim.CreateNewProcess(unitProcessName)
+    testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate))
+
+    # Construct algorithm and associated C++ container
+    platform = thrusterPlatformState.thrusterPlatformState()
+    platform.ModelTag = "platformReference"
+
+    # Add test module to runtime call list
+    unitTestSim.AddModelToTask(unitTaskName, platform)
+
+    # Initialize the test module configuration data
+    platform.sigma_MB = sigma_MB
+    platform.r_BM_M = r_BM_M
+    platform.r_FM_F = r_FM_F
+
+    # Create input THR Config Msg
+    THRConfig = messaging.THRConfigMsgPayload()
+    THRConfig.rThrust_B = r_TF_F
+    THRConfig.maxThrust = np.linalg.norm(T_F)
+    THRConfig.tHatThrust_B = T_F / THRConfig.maxThrust
+    thrConfigFMsg = messaging.THRConfigMsg().write(THRConfig)
+    platform.thrusterConfigFInMsg.subscribeTo(thrConfigFMsg)
+
+    # Create input hinged rigid body messages
+    hingedBodyMsg1 = messaging.HingedRigidBodyMsgPayload()
+    hingedBodyMsg1.theta = theta1
+    hingedBody1InMsg = messaging.HingedRigidBodyMsg().write(hingedBodyMsg1)
+    platform.hingedRigidBody1InMsg.subscribeTo(hingedBody1InMsg)
+    hingedBodyMsg2 = messaging.HingedRigidBodyMsgPayload()
+    hingedBodyMsg2.theta = theta2
+    hingedBody2InMsg = messaging.HingedRigidBodyMsg().write(hingedBodyMsg2)
+    platform.hingedRigidBody2InMsg.subscribeTo(hingedBody2InMsg)
+
+    # Setup logging on the test module output messages so that we get all the writes to it
+    thrConfigLog = platform.thrusterConfigBOutMsg.recorder()
+    unitTestSim.AddModelToTask(unitTaskName, thrConfigLog)
+
+    # Need to call the self-init and cross-init methods
+    unitTestSim.InitializeSimulation()
+
+    # Set the simulation time.
+    # NOTE: the total simulation time may be longer than this value. The
+    # simulation is stopped at the next logging event on or after the
+    # simulation end time.
+    unitTestSim.ConfigureStopTime(macros.sec2nano(0.5))        # seconds to stop simulation
+
+    # Begin the simulation time run set above
+    unitTestSim.ExecuteSimulation()
+
+    rThrust_B = thrConfigLog.rThrust_B[0]
+    tHatThrust_B = thrConfigLog.tHatThrust_B[0]
+    tMax = thrConfigLog.maxThrust[0]
+
+    FM = rbk.euler1232C([theta1, theta2, 0.0])
+    MB = rbk.MRP2C(sigma_MB)
+    FB = np.matmul(FM, MB)
+
+    r_TB_B = np.matmul(FB.transpose(), r_TF_F + r_FM_F - np.matmul(FM, r_BM_M))     # thrust application point
+    tHat_B = np.matmul(FB.transpose(), T_F) / np.linalg.norm(T_F)                   # thrust unit direction vector
+
+    np.testing.assert_allclose(rThrust_B, r_TB_B, rtol=0, atol=accuracy, verbose=True)
+    np.testing.assert_allclose(tHatThrust_B, tHat_B, rtol=0, atol=accuracy, verbose=True)
+    np.testing.assert_allclose(tMax, np.linalg.norm(T_F), rtol=0, atol=accuracy, verbose=True)
+
+    return
+
+
+#
+# This statement below ensures that the unitTestScript can be run as a
+# stand-along python script
+#
+if __name__ == "__main__":
+    test_platformRotation(
+                 False,                   # show_plots
+                 0,                       # theta1
+                 np.pi/36,                # theta2
+                 1e-10                    # accuracy
+    )

src/fswAlgorithms/effectorInterfaces/thrusterPlatformState/thrusterPlatformState.c

@@ -0,0 +1,99 @@
+/*
+ ISC License
+
+ Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
+
+ Permission to use, copy, modify, and/or distribute this software for any
+ purpose with or without fee is hereby granted, provided that the above
+ copyright notice and this permission notice appear in all copies.
+
+ THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+
+ */
+
+#include "thrusterPlatformState.h"
+#include <math.h>
+#include "architecture/utilities/linearAlgebra.h"
+#include "architecture/utilities/rigidBodyKinematics.h"
+
+/*! This method initializes the output messages for this module.
+ @return void
+ @param configData The configuration data associated with this module
+ @param moduleID The module identifier
+ */
+void SelfInit_thrusterPlatformState(thrusterPlatformStateConfig *configData, int64_t moduleID)
+{
+    THRConfigMsg_C_init(&configData->thrusterConfigBOutMsg);
+}
+
+
+/*! This method performs a complete reset of the module.  Local module variables that retain
+ time varying states between function calls are reset to their default values.
+ @return void
+ @param configData The configuration data associated with the module
+ @param callTime [ns] time the method is called
+ @param moduleID The module identifier
+*/
+void Reset_thrusterPlatformState(thrusterPlatformStateConfig *configData, uint64_t callTime, int64_t moduleID)
+{
+    if (!THRConfigMsg_C_isLinked(&configData->thrusterConfigFInMsg)) {
+        _bskLog(configData->bskLogger, BSK_ERROR, " thrusterPlatformState.thrusterConfigFInMsg wasn't connected.");
+    }
+    if (!HingedRigidBodyMsg_C_isLinked(&configData->hingedRigidBody1InMsg)) {
+        _bskLog(configData->bskLogger, BSK_ERROR, " thrusterPlatformState.hingedRigidBody1InMsg wasn't connected.");
+    }
+    if (!HingedRigidBodyMsg_C_isLinked(&configData->hingedRigidBody2InMsg)) {
+        _bskLog(configData->bskLogger, BSK_ERROR, " thrusterPlatformState.hingedRigidBody2InMsg wasn't connected.");
+    }
+}
+
+
+/*! This method updates the platformAngles message based on the updated information about the system center of mass
+ @return void
+ @param configData The configuration data associated with the module
+ @param callTime The clock time at which the function was called (nanoseconds)
+ @param moduleID The module identifier
+*/
+void Update_thrusterPlatformState(thrusterPlatformStateConfig *configData, uint64_t callTime, int64_t moduleID)
+{
+    /*! - Create and assign message buffers */
+    THRConfigMsgPayload        thrusterConfigFIn   = THRConfigMsg_C_read(&configData->thrusterConfigFInMsg);
+    HingedRigidBodyMsgPayload  hingedRigidBody1In = HingedRigidBodyMsg_C_read(&configData->hingedRigidBody1InMsg);
+    HingedRigidBodyMsgPayload  hingedRigidBody2In = HingedRigidBodyMsg_C_read(&configData->hingedRigidBody2InMsg);
+    THRConfigMsgPayload        thrusterConfigBOut  = THRConfigMsg_C_zeroMsgPayload();
+
+    /*! compute CM position w.r.t. M frame origin, in M coordinates */
+    double MB[3][3];
+    MRP2C(configData->sigma_MB, MB);                                 // B to M DCM
+    double r_TM_F[3];
+    v3Add(configData->r_FM_F, thrusterConfigFIn.rThrust_B, r_TM_F);   // position of T w.r.t. M in F-frame coordinates
+    double T_F[3];
+    v3Copy(thrusterConfigFIn.tHatThrust_B, T_F);
+    v3Scale(thrusterConfigFIn.maxThrust, T_F, T_F);
+
+    /*! extract theta1 and theta2 angles and compute FB DCM */
+    double EulerAngles123[3] = {hingedRigidBody1In.theta, hingedRigidBody2In.theta, 0.0};
+    double FM[3][3];
+    Euler1232C(EulerAngles123, FM);
+    double FB[3][3];
+    m33MultM33(FM, MB, FB);
+
+    /*! populate output msg */
+    double r_TM_B[3];
+    m33tMultV3(FB, r_TM_F, r_TM_B);
+    double r_BM_B[3];
+    m33tMultV3(MB, configData->r_BM_M, r_BM_B);
+    v3Subtract(r_TM_B, r_BM_B, thrusterConfigBOut.rThrust_B);
+    m33tMultV3(FB, thrusterConfigFIn.tHatThrust_B, thrusterConfigBOut.tHatThrust_B);
+    thrusterConfigBOut.maxThrust = thrusterConfigFIn.maxThrust;
+
+    /*! write output thruster config msg */
+    THRConfigMsg_C_write(&thrusterConfigBOut, &configData->thrusterConfigBOutMsg, moduleID, callTime);
+}
+

src/fswAlgorithms/effectorInterfaces/thrusterPlatformState/thrusterPlatformState.h

@@ -0,0 +1,63 @@
+/*
+ ISC License
+
+ Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
+
+ Permission to use, copy, modify, and/or distribute this software for any
+ purpose with or without fee is hereby granted, provided that the above
+ copyright notice and this permission notice appear in all copies.
+
+ THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+
+ */
+
+#ifndef _THRUSTER_PLATFORM_STATE_
+#define _THRUSTER_PLATFORM_STATE_
+
+#include <stdint.h>
+#include "architecture/utilities/bskLogging.h"
+#include "cMsgCInterface/CmdTorqueBodyMsg_C.h"
+#include "cMsgCInterface/HingedRigidBodyMsg_C.h"
+#include "cMsgCInterface/THRConfigMsg_C.h"
+
+
+/*! @brief Top level structure for the sub-module routines. */
+typedef struct {
+
+    /* declare these user-defined quantities */
+    double sigma_MB[3];                                   //!< orientation of the M frame w.r.t. the B frame
+    double r_BM_M[3];                                     //!< position of B frame origin w.r.t. M frame origin, in M frame coordinates
+    double r_FM_F[3];                                     //!< position of F frame origin w.r.t. M frame origin, in F frame coordinates
+
+    double K;                                             //!< momentum dumping time constant [1/s]
+
+    /* declare module IO interfaces */
+    THRConfigMsg_C            thrusterConfigFInMsg;       //!< input thruster configuration msg
+    HingedRigidBodyMsg_C      hingedRigidBody1InMsg;      //!< output msg containing theta1 reference and thetaDot1 reference
+    HingedRigidBodyMsg_C      hingedRigidBody2InMsg;      //!< output msg containing theta2 reference and thetaDot2 reference
+    THRConfigMsg_C            thrusterConfigBOutMsg;      //!< output msg containing the thruster configuration infor in B-frame
+
+    BSKLogger *bskLogger;                                 //!< BSK Logging
+
+}thrusterPlatformStateConfig;
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    void SelfInit_thrusterPlatformState(thrusterPlatformStateConfig *configData, int64_t moduleID);
+    void Reset_thrusterPlatformState(thrusterPlatformStateConfig *configData, uint64_t callTime, int64_t moduleID);
+    void Update_thrusterPlatformState(thrusterPlatformStateConfig *configData, uint64_t callTime, int64_t moduleID);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif

src/fswAlgorithms/effectorInterfaces/thrusterPlatformState/thrusterPlatformState.i

@@ -0,0 +1,35 @@
+/*
+ ISC License
+
+ Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
+
+ Permission to use, copy, modify, and/or distribute this software for any
+ purpose with or without fee is hereby granted, provided that the above
+ copyright notice and this permission notice appear in all copies.
+
+ THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+
+ */
+%module thrusterPlatformState
+%{
+   #include "thrusterPlatformState.h"
+%}
+
+%include "swig_c_wrap.i"
+%c_wrap(thrusterPlatformState);
+
+%include "architecture/msgPayloadDefC/THRConfigMsgPayload.h"
+struct THRConfigMsg_C;
+%include "architecture/msgPayloadDefC/HingedRigidBodyMsgPayload.h"
+struct HingedRigidBodyMsg_C;
+
+%pythoncode %{
+import sys
+protectAllClasses(sys.modules[__name__])
+%}