diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/_UnitTest/test_spinningBodyOneDOFStateEffector.py b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/_UnitTest/test_spinningBodyOneDOFStateEffector.py index a00455db57..6dde7d18e3 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/_UnitTest/test_spinningBodyOneDOFStateEffector.py +++ b/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) diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp index 26a17d4194..97b90e4f48 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.cpp +++ b/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,6 +139,11 @@ 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) @@ -135,11 +151,20 @@ 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 diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h index 7525b54fa4..a1d7368578 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.h +++ b/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::infinity(); //!< [rad] Maximum allowed angle + double theta_min = -std::numeric_limits::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 diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.i b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.i index abe3e6db72..1c9e193fa7 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.i +++ b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.i @@ -16,7 +16,17 @@ OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. */ - +#include + +%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 %{ diff --git a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.rst b/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.rst index 66827d7890..1e64398670 100644 --- a/src/simulation/dynamics/spinningBodies/spinningBodiesOneDOF/spinningBodyOneDOFStateEffector.rst +++ b/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``.