Implement hardstops for spinningBodiesOneDOF

src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/_UnitTest/test_spinningBodyOneDOFStateEffector.py

@@ -296,5 +296,141 @@ def spinningBody(show_plots, cmdTorque, lock, thetaRef):
     return [testFailCount, ''.join(testMessages)]
 
 
+@pytest.mark.parametrize(
+    "max_lim, min_lim", [(-70, 70), (-10, -20)]  # cases: max less than min, start angle inside bounds
+)
+def test_spinning_body_enforces_limits(max_lim, min_lim):
+    """Verify that model raises exception when invalid limits are specified.
+
+    :param float max_lim: Maximum angle limit [deg]
+    :param float min_lim: Minimum angle limit [deg]
+    """
+    spinningBody = spinningBodyOneDOFStateEffector.SpinningBodyOneDOFStateEffector()
+    spinningBody.sHat_S = [0, -1, 0]
+    spinningBody.theta_max = np.deg2rad(max_lim)
+    spinningBody.theta_min = np.deg2rad(min_lim)
+
+    with pytest.raises(ValueError) as exc:
+        spinningBody.Reset(0)
+
+
+@pytest.mark.parametrize(
+    "max_lim, min_lim",
+    [(70, -70), (20, -10), (0, -20)],  # cases: symmetric range, asymmetric range, range including start position
+)
+def test_spinning_body_limits(show_plots, max_lim, min_lim):
+    """Verify that model stops motion at limits.
+
+    :param bool show_plots: Display plots if True
+    :param float max_lim: Maximum angle limit [deg]
+    :param float min_lim: Minimum angle limit [deg]
+    """
+    unitTaskName = "unitTask"  # arbitrary name (don't change)
+    unitProcessName = "TestProcess"  # arbitrary name (don't change)
+
+    #   Create a sim module as an empty container
+    unitTestSim = SimulationBaseClass.SimBaseClass()
+
+    # Create test thread
+    testProcessRate = macros.sec2nano(0.1)  # update process rate update time
+    testProc = unitTestSim.CreateNewProcess(unitProcessName)
+    testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate))
+
+    # Create the spacecraft module
+    scObject = spacecraft.Spacecraft()
+    scObject.ModelTag = "spacecraftBody"
+
+    # Define mass properties of the rigid hub of the spacecraft
+    scObject.hub.mHub = 750.0
+    scObject.hub.r_BcB_B = [0.0, 0.0, 0.0]
+    scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]]
+    unitTestSim.AddModelToTask(unitTaskName, scObject)
+
+    # Create a spinning body
+    spinningBody = spinningBodyOneDOFStateEffector.SpinningBodyOneDOFStateEffector()
+    spinningBody.mass = 50.0
+    spinningBody.IPntSc_S = [[50.0, 0.0, 0.0], [0.0, 30.0, 0.0], [0.0, 0.0, 40.0]]
+    spinningBody.dcm_S0B = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
+    spinningBody.r_ScS_S = [[1.0], [0.0], [-1.0]]
+    spinningBody.r_SB_B = [[0.5], [-1.5], [-0.5]]
+    spinningBody.sHat_S = [[0], [-1], [0]]
+    spinningBody.theta_max = np.deg2rad(max_lim)
+    spinningBody.theta_min = np.deg2rad(min_lim)
+    spinningBody.ModelTag = "SpinningBody"
+    unitTestSim.AddModelToTask(unitTaskName, spinningBody)
+    scObject.addStateEffector(spinningBody)
+
+    # Create the torque message
+    cmdArray = messaging.ArrayMotorTorqueMsgPayload()
+    cmdArray.motorTorque = [1] + [0] * 5  # [Nm]
+    cmdMsg = messaging.ArrayMotorTorqueMsg().write(cmdArray)
+    spinningBody.motorTorqueInMsg.subscribeTo(cmdMsg)
+
+    # Initialize the simulation
+    unitTestSim.InitializeSimulation()
+
+    # Setup logging
+    spinning_body_log = spinningBody.spinningBodyOutMsg.recorder()
+    unitTestSim.AddModelToTask(unitTaskName, spinning_body_log)
+    sc_log = scObject.scStateOutMsg.recorder()
+    unitTestSim.AddModelToTask(unitTaskName, sc_log)
+    torque_log = cmdMsg.recorder()
+    unitTestSim.AddModelToTask(unitTaskName, torque_log)
+
+    # Setup and run the simulation
+    stopTime = 200 * testProcessRate
+    unitTestSim.ConfigureStopTime(stopTime)
+    unitTestSim.ExecuteSimulation()
+
+    # run with opposite torque
+    cmdArray.motorTorque = [-1] + [0] * 5  # [Nm]
+    cmdMsg = cmdMsg.write(cmdArray)
+    scObject.hub.omega_BN_BInit = [0.0, 0.0, 0.0]
+
+    unitTestSim.ConfigureStopTime(3 * stopTime)  # run twice as long to ensure body reaches opposite side
+    unitTestSim.ExecuteSimulation()
+
+    # Extract the logged variables
+    time_sec = np.array(sc_log.times()) * macros.NANO2SEC
+    theta = spinning_body_log.theta
+    torque = torque_log.motorTorque[:, 0]
+    thetaDot = spinning_body_log.thetaDot
+    omega = sc_log.omega_BN_B
+
+    fig = plt.figure()
+
+    ax = fig.subplots(ncols=1, nrows=4, height_ratios=[2, 2, 1, 1])
+    ax[0].plot(time_sec, np.rad2deg(theta))
+    ax[0].axhline(y=max_lim, color="r")
+    ax[0].axhline(y=min_lim, color="r", label="Limit")
+    ax[0].set_ylabel("Theta [deg]")
+
+    ax[1].plot(time_sec, np.rad2deg(thetaDot), label="Rate")
+    ax[1].set_ylabel("Rate [deg/s]")
+
+    ax[2].plot(time_sec, torque, label="Torque")
+    ax[2].set_ylabel("Torque [N]")
+
+    ax[3].plot(time_sec, np.rad2deg(omega), label="omega_BN_B")
+    ax[3].plot(time_sec, np.rad2deg(np.linalg.norm(omega, axis=1)), linestyle="dashed", label="Norm")
+    ax[3].set_xlabel("Time [s]")
+    ax[3].set_ylabel("omega_BN_B [deg/s]")
+    for axis in ax:
+        axis.grid()
+    plt.suptitle("Spinning Body Response w/Limits")
+
+    if show_plots:
+        plt.show()
+    plt.close("all")
+
+    # ensure that body never exceeded limits
+    assert np.all(theta <= np.deg2rad(max_lim))
+    assert np.all(theta >= np.deg2rad(min_lim))
+
+    # but also ensure that body reached the intended limits
+    assert np.any(np.abs(theta - np.deg2rad(max_lim)) < 1e-4)
+    assert np.any(np.abs(theta - np.deg2rad(min_lim)) < 1e-4)
+
+
 if __name__ == "__main__":
     spinningBody(True, 0.0, False, 0.0 * macros.D2R)

src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp

@@ -68,6 +68,17 @@ void SpinningBodyOneDOFStateEffector::Reset(uint64_t CurrentClock)
     else {
         bskLogger.bskLog(BSK_ERROR, "Norm of sHat must be greater than 0. sHat may not have been set by the user.");
     }
+    // Ensure user specified valid angular limits
+    if (this->theta_max < this->theta_min) {
+        throw std::invalid_argument("theta_max (" + std::to_string(this->theta_max)
+            + ") must be greater than theta_min (" + std::to_string(this->theta_min) + ").");
+    }
+    // Ensure that user specified valid initial angle
+    if ((this->thetaInit > this->theta_max) || (this->thetaInit < this->theta_min)) {
+        throw std::invalid_argument("Initial angle (" + std::to_string(this->thetaInit)
+            + ") must be inside of body angle bounds (" + std::to_string(this->theta_min) + ", "
+            + std::to_string(this->theta_max) + ").");
+    }
 }
 
 
@@ -128,18 +139,32 @@ void SpinningBodyOneDOFStateEffector::registerStates(DynParamManager& states)
     this->thetaDotState->setState(thetaDotInitMatrix);
 }
 
+// Determine if body is attempting to move beyond specified limits.
+bool SpinningBodyOneDOFStateEffector::isMovingBeyondLimits(double theta, double thetaDot) {
+    return (((theta >= this->theta_max) && (thetaDot > 0)) || ((theta <= this->theta_min) && (thetaDot < 0)));
+}
+
 /*! This method allows the SB state effector to provide its contributions to the mass props and mass prop rates of the
  spacecraft */
 void SpinningBodyOneDOFStateEffector::updateEffectorMassProps(double integTime)
 {
     // Give the mass of the spinning body to the effProps mass
     this->effProps.mEff = this->mass;
 
-    // Lock the axis if the flag is set to 1
-    if (this->lockFlag == 1)
-    {
+    // Lock the axis if the flag is set to 1 or attempting to move beyond bounds
+    if (this->lockFlag == 1
+        || this->isMovingBeyondLimits(this->thetaState->getState()(0, 0), this->thetaDotState->getState()(0, 0))) {
         Eigen::MatrixXd zeroMatrix = Eigen::MatrixXd::Constant(1, 1, 0.0);
         this->thetaDotState->setState(zeroMatrix);
+        // Zeroing thetaDot (just above) won't stop body exactly at limit (depending on timestep),
+        // so set the theta state to be exactly at the limit
+        if (this->theta >= this->theta_max) {
+            Eigen::MatrixXd limMatrix = Eigen::MatrixXd::Constant(1, 1, this->theta_max);
+            this->thetaState->setState(limMatrix);
+        } else if (this->theta <= this->theta_min) {
+            Eigen::MatrixXd limMatrix = Eigen::MatrixXd::Constant(1, 1, this->theta_min);
+            this->thetaState->setState(limMatrix);
+        }
     }
 
     // Grab current states
@@ -232,6 +257,16 @@ void SpinningBodyOneDOFStateEffector::updateContributions(double integTime,
                 + this->sHat_B.dot(gravityTorquePntS_B - omegaTilde_SN_B * IPntS_B * this->omega_SN_B
                 - IPntS_B * this->omegaTilde_BN_B * this->omega_SB_B
                 - this->mass * rTilde_ScS_B * this->omegaTilde_BN_B * rDot_SB_B)) / this->mTheta;
+
+        // After computing interations between body and hub, if hub is moving
+        // further against limits, zero out the motion to simulate the physical
+        // barrier there.
+        if ((this->theta == this->theta_max && this->cTheta > 0)
+            || (this->theta == this->theta_min && this->cTheta < 0)) {
+            this->aTheta.setZero();
+            this->bTheta.setZero();
+            this->cTheta = 0.0;
+        }
     }
 
     // For documentation on contributions see Vaz Carneiro, Allard, Schaub spinning body paper

src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h

@@ -43,6 +43,8 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel {
     double c = 0.0;                                                  //!< [N-m-s/rad] rotational damping coefficient
     double thetaInit = 0.0;                                          //!< [rad] initial spinning body angle
     double thetaDotInit = 0.0;                                       //!< [rad/s] initial spinning body angle rate
+    double theta_max = std::numeric_limits<double>::infinity();      //!< [rad] Maximum allowed angle
+    double theta_min = -std::numeric_limits<double>::infinity();     //!< [rad] Minimum allowed angle
     std::string nameOfThetaState;                                    //!< -- identifier for the theta state data container
     std::string nameOfThetaDotState;                                 //!< -- identifier for the thetaDot state data container
     Eigen::Vector3d r_SB_B{0.0, 0.0, 0.0};                  //!< [m] vector pointing from body frame B origin to spinning frame S origin in B frame components
@@ -75,6 +77,8 @@ class SpinningBodyOneDOFStateEffector: public StateEffector, public SysModel {
     void computeSpinningBodyInertialStates();               //!< Method for computing the SB's states
 
 private:
+    bool isMovingBeyondLimits(double theta, double thetaDot);
+
     static uint64_t effectorID;         //!< [] ID number of this panel
     double u = 0.0;                     //!< [N-m] optional motor torque
     int lockFlag = 0;                   //!< [] flag for locking the rotation axis

src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.i

@@ -16,7 +16,17 @@
  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 
  */
-
+#include <stdexcept>
+
+%exception { 
+    try {
+        $action
+    } catch (std::invalid_argument &e) {
+        std::string s("spinningBodyOneDOF error: "), s2(e.what());
+        s = s + s2;
+        SWIG_exception(SWIG_ValueError, s.c_str());
+   }
+}
 
 %module spinningBodyOneDOFStateEffector
 %{

src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.rst

@@ -107,6 +107,11 @@ This section is to outline the steps needed to setup a Spinning Body State Effec
     angleRefMsg = messaging.HingedRigidBodyMsg().write(angleRef)
     spinningBody.spinningBodyRefInMsg.subscribeTo(angleRefMsg)
 
+#. (Optional) Specify angular limits for the body's rotation::
+
+    spinningBody.theta_max = 90.0 * macros.D2R
+    spinningBody.theta_min = -90.0 * macros.D2R
+
 #. The angular states of the body are created using an output message ``spinningBodyOutMsg``.
 
 #. The spinning body config log state output message is ``spinningBodyConfigLogOutMsg``.