Source code for test_linearTranslationOneDOFStateEffector

# ISC License
#
# Copyright (c) 2024, 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.


#
#   Unit Test Script
#   Module Name:        translatingBodies
#   Author:             Peter Johnson
#   Creation Date:      March 6, 2024
#

import inspect
import os
import matplotlib.pyplot as plt
import numpy
import numpy as np
import pytest

filename = inspect.getframeinfo(inspect.currentframe()).filename
path = os.path.dirname(os.path.abspath(filename))
splitPath = path.split('simulation')

from Basilisk.utilities import SimulationBaseClass, unitTestSupport, macros
from Basilisk.simulation import spacecraft, linearTranslationOneDOFStateEffector, gravityEffector
from Basilisk.architecture import messaging


# uncomment this line if this test is to be skipped in the global unit test run, adjust message as needed
# @pytest.mark.skipif(conditionstring)
# uncomment this line if this test has an expected failure, adjust message as needed
# @pytest.mark.xfail()
# provide a unique test method name, starting with test_


# tests are paramterized by four functions
[docs] @pytest.mark.parametrize("function", ["translatingBodyNoInput" , "translatingBodyLockFlag" , "translatingBodyCommandedForce" , "translatingBodyRhoReference" ]) def test_translatingBody(show_plots, function): r""" **Validation Test Description** This unit test sets up a spacecraft with a single-axis translating rigid body attached to a rigid hub. The position of the boom axis is arbitrary. The scenario includes gravity acting on both the spacecraft and the effector. **Description of Variables Being Tested** In this file we are checking the principles of conservation of energy and angular momentum. Both the orbital and rotational energy and angular momentum must be maintained when conservative forces like gravity are present. Therefore, the values of the variables - ``finalOrbAngMom`` - ``finalOrbEnergy`` - ``finalRotAngMom`` - ``finalRotEnergy`` should be constant when tested against their initial values. """ if function == "translatingBodyCommandedForce": eval(function + '(show_plots, 1.0)') elif function == "translatingBodyRhoReference": eval(function + '(show_plots, 0.5)') else: eval(function + '(show_plots)')
# rho ref and cmd force are zero, no lock flag def translatingBodyNoInput(show_plots): __tracebackhide__ = True testFailCount = 0 # zero unit test result counter testMessages = [] # create empty list to store test log messages 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.001) # 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], [1.0]] scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] # Set the initial values for the states scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] scObject.hub.omega_BN_BInit = [[0.1], [-0.1], [0.1]] # Create a linear translating effector translatingBody = linearTranslationOneDOFStateEffector.linearTranslationOneDOFStateEffector() # Define properties of translating body mass = 20.0 rhoInit = 1.0 rhoDotInit = 0.05 fHat_B = [[3.0 / 5.0], [4.0 / 5.0], [0.0]] r_FcF_F = [[-1.0], [1.0], [0.0]] r_F0B_B = [[-5.0], [4.0], [3.0]] IPntFc_F = [[50.0, 0.0, 0.0], [0.0, 80.0, 0.0], [0.0, 0.0, 60.0]] dcm_FB = [[0.0, -1.0, 0.0], [0.0, 0.0, -1.0], [1.0, 0.0, 0.0]] k = 100.0 c = 0 # set parameters above translatingBody.setMass(mass) translatingBody.setK(k) translatingBody.setC(c) translatingBody.setRhoInit(rhoInit) translatingBody.setRhoDotInit(rhoDotInit) translatingBody.setFHat_B(fHat_B) translatingBody.setR_FcF_F(r_FcF_F) translatingBody.setR_F0B_B(r_F0B_B) translatingBody.setIPntFc_F(IPntFc_F) translatingBody.setDCM_FB(dcm_FB) translatingBody.ModelTag = "translatingBody" # Add translating body to spacecraft scObject.addStateEffector(translatingBody) # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, translatingBody) unitTestSim.AddModelToTask(unitTaskName, scObject) # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" earthGravBody.mu = 0.3986004415E+15 # meters! earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) # Log the spacecraft state message datLog = scObject.scStateOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, datLog) # Add energy and momentum variables to log scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Initialize the simulation unitTestSim.InitializeSimulation() # Add states to log rhoData = translatingBody.translatingBodyOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, rhoData) # Setup and run the simulation stopTime = 25000 * testProcessRate unitTestSim.ConfigureStopTime(stopTime) unitTestSim.ExecuteSimulation() # Extract the logged variables orbAngMom_N = scObjectLog.totOrbAngMomPntN_N rotAngMom_N = scObjectLog.totRotAngMomPntC_N rotEnergy = scObjectLog.totRotEnergy orbEnergy = scObjectLog.totOrbEnergy rho = rhoData.rho rhoDot = rhoData.rhoDot # Set up the conservation quantities timeSec = scObjectLog.times() * 1e-9 initialOrbAngMom_N = [orbAngMom_N[0, 0], orbAngMom_N[0, 1], orbAngMom_N[0, 2]] finalOrbAngMom = orbAngMom_N[-1] initialRotAngMom_N = [rotAngMom_N[0, 0], rotAngMom_N[0, 1], rotAngMom_N[0, 2]] finalRotAngMom = rotAngMom_N[-1] initialOrbEnergy = orbEnergy[0] finalOrbEnergy = orbEnergy[-1] initialRotEnergy = rotEnergy[0] finalRotEnergy = rotEnergy[-1] # Plotting plt.close("all") plt.figure() plt.clf() plt.plot(timeSec, (orbAngMom_N[:, 0] - initialOrbAngMom_N[0]) / initialOrbAngMom_N[0], timeSec, (orbAngMom_N[:, 1] - initialOrbAngMom_N[1]) / initialOrbAngMom_N[1], timeSec, (orbAngMom_N[:, 2] - initialOrbAngMom_N[2]) / initialOrbAngMom_N[2]) plt.xlabel('time (s)') plt.ylabel('Relative Difference') plt.title('Orbital Angular Momentum') plt.figure() plt.clf() plt.plot(timeSec, (orbEnergy - initialOrbEnergy) / initialOrbEnergy) plt.xlabel('time (s)') plt.ylabel('Relative Difference') plt.title('Orbital Energy') plt.figure() plt.clf() plt.plot(timeSec, (rotAngMom_N[:, 0] - initialRotAngMom_N[0]) / initialRotAngMom_N[0], timeSec, (rotAngMom_N[:, 1] - initialRotAngMom_N[1]) / initialRotAngMom_N[1], timeSec, (rotAngMom_N[:, 2] - initialRotAngMom_N[2]) / initialRotAngMom_N[2]) plt.xlabel('time (s)') plt.ylabel('Relative Difference') plt.title('Rotational Angular Momentum') plt.figure() plt.clf() plt.plot(timeSec, (rotEnergy - initialRotEnergy) / initialRotEnergy) plt.xlabel('time (s)') plt.ylabel('Relative Difference') plt.title('Rotational Energy') plt.figure() plt.clf() plt.plot(timeSec, rho) plt.xlabel('time (s)') plt.ylabel('rho') plt.figure() plt.clf() plt.plot(timeSec, rhoDot) plt.xlabel('time (s)') plt.ylabel('rhoDot') if show_plots: plt.show() plt.close("all") # Testing setup accuracy = 1e-12 np.testing.assert_allclose(finalOrbEnergy, initialOrbEnergy, rtol=accuracy, err_msg="Orbital energy is not constant.") np.testing.assert_allclose(finalRotEnergy, initialRotEnergy, rtol=accuracy, err_msg="Rotational energy is not constant.") for i in range(3): np.testing.assert_allclose(finalOrbAngMom, initialOrbAngMom_N, rtol=accuracy, err_msg="Orbital angular momentum is not constant.") np.testing.assert_allclose(finalRotAngMom, initialRotAngMom_N, rtol=accuracy, err_msg="Rotational angular momentum is not constant.") # rho ref and cmd force are zero, lock flag is enabled def translatingBodyLockFlag(show_plots): __tracebackhide__ = True testFailCount = 0 # zero unit test result counter testMessages = [] # create empty list to store test log messages 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.001) # 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], [1.0]] scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] # Set the initial values for the states scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] scObject.hub.omega_BN_BInit = [[0.1], [-0.1], [0.1]] # Create a linear translating effector translatingBody = linearTranslationOneDOFStateEffector.linearTranslationOneDOFStateEffector() # Define properties of translating body mass = 20.0 rhoInit = 1.0 rhoDotInit = 0 fHat_B = [[3.0 / 5.0], [4.0 / 5.0], [0.0]] r_FcF_F = [[-1.0], [1.0], [0.0]] r_F0B_B = [[-5.0], [4.0], [3.0]] IPntFc_F = [[50.0, 0.0, 0.0], [0.0, 80.0, 0.0], [0.0, 0.0, 60.0]] dcm_FB = [[0.0, -1.0, 0.0], [0.0, 0.0, -1.0], [1.0, 0.0, 0.0]] k = 100.0 c = 0 # set parameters above translatingBody.setMass(mass) translatingBody.setK(k) translatingBody.setC(c) translatingBody.setRhoInit(rhoInit) translatingBody.setRhoDotInit(rhoDotInit) translatingBody.setFHat_B(fHat_B) translatingBody.setR_FcF_F(r_FcF_F) translatingBody.setR_F0B_B(r_F0B_B) translatingBody.setIPntFc_F(IPntFc_F) translatingBody.setDCM_FB(dcm_FB) translatingBody.ModelTag = "translatingBody" # Add translating body to spacecraft scObject.addStateEffector(translatingBody) # create lock message lockArray = messaging.ArrayEffectorLockMsgPayload() lockArray.effectorLockFlag = [1] lockMsg = messaging.ArrayEffectorLockMsg().write(lockArray) translatingBody.motorLockInMsg.subscribeTo(lockMsg) # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, translatingBody) unitTestSim.AddModelToTask(unitTaskName, scObject) # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" earthGravBody.mu = 0.3986004415E+15 # meters! earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) # Log the spacecraft state message datLog = scObject.scStateOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, datLog) # Add energy and momentum variables to log scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy", "totRotEnergy"]) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Initialize the simulation unitTestSim.InitializeSimulation() # Add states to log rhoData = translatingBody.translatingBodyOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, rhoData) # Setup and run the simulation stopTime = 25000 * testProcessRate unitTestSim.ConfigureStopTime(stopTime) unitTestSim.ExecuteSimulation() # Extract the logged variables orbAngMom_N = scObjectLog.totOrbAngMomPntN_N rotAngMom_N = scObjectLog.totRotAngMomPntC_N rotEnergy = scObjectLog.totRotEnergy orbEnergy = scObjectLog.totOrbEnergy rho = rhoData.rho rhoDot = rhoData.rhoDot # Set up the conservation quantities timeSec = scObjectLog.times() * 1e-9 initialOrbAngMom_N = [orbAngMom_N[0, 0], orbAngMom_N[0, 1], orbAngMom_N[0, 2]] finalOrbAngMom = orbAngMom_N[-1] initialRotAngMom_N = [rotAngMom_N[0, 0], rotAngMom_N[0, 1], rotAngMom_N[0, 2]] finalRotAngMom = rotAngMom_N[-1] initialOrbEnergy = orbEnergy[0] finalOrbEnergy = orbEnergy[-1] initialRotEnergy = rotEnergy[0] finalRotEnergy = rotEnergy[-1] # Plotting plt.close("all") plt.figure() plt.clf() plt.plot(timeSec, (orbAngMom_N[:, 0] - initialOrbAngMom_N[0]) / initialOrbAngMom_N[0], timeSec, (orbAngMom_N[:, 1] - initialOrbAngMom_N[1]) / initialOrbAngMom_N[1], timeSec, (orbAngMom_N[:, 2] - initialOrbAngMom_N[2]) / initialOrbAngMom_N[2]) plt.xlabel('time (s)') plt.ylabel('Relative Difference') plt.title('Orbital Angular Momentum') plt.figure() plt.clf() plt.plot(timeSec, (orbEnergy - initialOrbEnergy) / initialOrbEnergy) plt.xlabel('time (s)') plt.ylabel('Relative Difference') plt.title('Orbital Energy') plt.figure() plt.clf() plt.plot(timeSec, (rotAngMom_N[:, 0] - initialRotAngMom_N[0]) / initialRotAngMom_N[0], timeSec, (rotAngMom_N[:, 1] - initialRotAngMom_N[1]) / initialRotAngMom_N[1], timeSec, (rotAngMom_N[:, 2] - initialRotAngMom_N[2]) / initialRotAngMom_N[2]) plt.xlabel('time (s)') plt.ylabel('Relative Difference') plt.title('Rotational Angular Momentum') plt.figure() plt.clf() plt.plot(timeSec, (rotEnergy - initialRotEnergy) / initialRotEnergy) plt.xlabel('time (s)') plt.ylabel('Relative Difference') plt.title('Rotational Energy') plt.figure() plt.clf() plt.plot(timeSec, rho) plt.xlabel('time (s)') plt.ylabel('rho') plt.figure() plt.clf() plt.plot(timeSec, rhoDot) plt.xlabel('time (s)') plt.ylabel('rhoDot') if show_plots: plt.show() plt.close("all") # Testing setup accuracy = 1e-12 np.testing.assert_allclose(finalOrbEnergy, initialOrbEnergy, rtol=accuracy, err_msg="Orbital energy is not constant.") np.testing.assert_allclose(finalRotEnergy, initialRotEnergy, rtol=accuracy, err_msg="Rotational energy is not constant.") for i in range(3): np.testing.assert_allclose(finalOrbAngMom, initialOrbAngMom_N, rtol=accuracy, err_msg="Orbital angular momentum is not constant.") np.testing.assert_allclose(finalRotAngMom, initialRotAngMom_N, rtol=accuracy, err_msg="Rotational angular momentum is not constant.") # cmd force is nonzero, rho ref is zero, no lock flag def translatingBodyCommandedForce(show_plots, cmdForce): __tracebackhide__ = True testFailCount = 0 # zero unit test result counter testMessages = [] # create empty list to store test log messages 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.001) # 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], [1.0]] scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] # Set the initial values for the states scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] scObject.hub.omega_BN_BInit = [[0.1], [-0.1], [0.1]] # Create a linear translating effector translatingBody = linearTranslationOneDOFStateEffector.linearTranslationOneDOFStateEffector() # Define properties of translating body mass = 20.0 rhoInit = 1.0 rhoDotInit = 0.05 fHat_B = [[3.0 / 5.0], [4.0 / 5.0], [0.0]] r_FcF_F = [[-1.0], [1.0], [0.0]] r_F0B_B = [[-5.0], [4.0], [3.0]] IPntFc_F = [[50.0, 0.0, 0.0], [0.0, 80.0, 0.0], [0.0, 0.0, 60.0]] dcm_FB = [[0.0, -1.0, 0.0], [0.0, 0.0, -1.0], [1.0, 0.0, 0.0]] k = 100.0 c = 0 # set parameters above translatingBody.setMass(mass) translatingBody.setK(k) translatingBody.setC(c) translatingBody.setRhoInit(rhoInit) translatingBody.setRhoDotInit(rhoDotInit) translatingBody.setFHat_B(fHat_B) translatingBody.setR_FcF_F(r_FcF_F) translatingBody.setR_F0B_B(r_F0B_B) translatingBody.setIPntFc_F(IPntFc_F) translatingBody.setDCM_FB(dcm_FB) translatingBody.ModelTag = "translatingBody" # Add translating body to spacecraft scObject.addStateEffector(translatingBody) # Create the force cmd force message cmdArray = messaging.ArrayMotorForceMsgPayload() cmdArray.motorForce = [cmdForce] # [Nm] cmdMsg = messaging.ArrayMotorForceMsg().write(cmdArray) translatingBody.motorForceInMsg.subscribeTo(cmdMsg) # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, translatingBody) unitTestSim.AddModelToTask(unitTaskName, scObject) # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" earthGravBody.mu = 0.3986004415E+15 # meters! earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) # Log the spacecraft state message datLog = scObject.scStateOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, datLog) # Add energy and momentum variables to log scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy"]) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Initialize the simulation unitTestSim.InitializeSimulation() # Add states to log rhoData = translatingBody.translatingBodyOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, rhoData) # Setup and run the simulation stopTime = 25000 * testProcessRate unitTestSim.ConfigureStopTime(stopTime) unitTestSim.ExecuteSimulation() # Extract the logged variables orbAngMom_N = scObjectLog.totOrbAngMomPntN_N rotAngMom_N = scObjectLog.totRotAngMomPntC_N orbEnergy = scObjectLog.totOrbEnergy rho = rhoData.rho rhoDot = rhoData.rhoDot # Set up the conservation quantities timeSec = scObjectLog.times() * 1e-9 initialOrbAngMom_N = [orbAngMom_N[0, 0], orbAngMom_N[0, 1], orbAngMom_N[0, 2]] finalOrbAngMom = orbAngMom_N[-1] initialRotAngMom_N = [rotAngMom_N[0, 0], rotAngMom_N[0, 1], rotAngMom_N[0, 2]] finalRotAngMom = rotAngMom_N[-1] initialOrbEnergy = orbEnergy[0] finalOrbEnergy = orbEnergy[-1] # Plotting plt.close("all") plt.figure() plt.clf() plt.plot(timeSec, (orbAngMom_N[:, 0] - initialOrbAngMom_N[0]) / initialOrbAngMom_N[0], timeSec, (orbAngMom_N[:, 1] - initialOrbAngMom_N[1]) / initialOrbAngMom_N[1], timeSec, (orbAngMom_N[:, 2] - initialOrbAngMom_N[2]) / initialOrbAngMom_N[2]) plt.xlabel('time (s)') plt.ylabel('Relative Difference') plt.title('Orbital Angular Momentum') plt.figure() plt.clf() plt.plot(timeSec, (orbEnergy - initialOrbEnergy) / initialOrbEnergy) plt.xlabel('time (s)') plt.ylabel('Relative Difference') plt.title('Orbital Energy') plt.figure() plt.clf() plt.plot(timeSec, (rotAngMom_N[:, 0] - initialRotAngMom_N[0]) / initialRotAngMom_N[0], timeSec, (rotAngMom_N[:, 1] - initialRotAngMom_N[1]) / initialRotAngMom_N[1], timeSec, (rotAngMom_N[:, 2] - initialRotAngMom_N[2]) / initialRotAngMom_N[2]) plt.xlabel('time (s)') plt.ylabel('Relative Difference') plt.title('Rotational Angular Momentum') plt.figure() plt.clf() plt.plot(timeSec, rho) plt.xlabel('time (s)') plt.ylabel('rho') plt.figure() plt.clf() plt.plot(timeSec, rhoDot) plt.xlabel('time (s)') plt.ylabel('rhoDot') if show_plots: plt.show() plt.close("all") # Testing setup accuracy = 1e-12 np.testing.assert_allclose(finalOrbEnergy, initialOrbEnergy, rtol=accuracy, err_msg="Orbital energy is not constant.") for i in range(3): np.testing.assert_allclose(finalOrbAngMom, initialOrbAngMom_N, rtol=accuracy, err_msg="Orbital angular momentum is not constant.") np.testing.assert_allclose(finalRotAngMom, initialRotAngMom_N, rtol=accuracy, err_msg="Rotational angular momentum is not constant.") # rho ref is nonzero, cmd force is zero and lock flag is false def translatingBodyRhoReference(show_plots, rhoRef): __tracebackhide__ = True testFailCount = 0 # zero unit test result counter testMessages = [] # create empty list to store test log messages 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.001) # 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], [1.0]] scObject.hub.IHubPntBc_B = [[900.0, 0.0, 0.0], [0.0, 800.0, 0.0], [0.0, 0.0, 600.0]] # Set the initial values for the states scObject.hub.r_CN_NInit = [[-4020338.690396649], [7490566.741852513], [5248299.211589362]] scObject.hub.v_CN_NInit = [[-5199.77710904224], [-3436.681645356935], [1041.576797498721]] scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]] scObject.hub.omega_BN_BInit = [[0.1], [-0.1], [0.1]] # Create a linear translating effector translatingBody = linearTranslationOneDOFStateEffector.linearTranslationOneDOFStateEffector() # Define properties of translating body mass = 20.0 rhoInit = 1.0 rhoDotInit = 0.05 fHat_B = [[3.0 / 5.0], [4.0 / 5.0], [0.0]] r_FcF_F = [[-1.0], [1.0], [0.0]] r_F0B_B = [[-5.0], [4.0], [3.0]] IPntFc_F = [[50.0, 0.0, 0.0], [0.0, 80.0, 0.0], [0.0, 0.0, 60.0]] dcm_FB = [[0.0, -1.0, 0.0], [0.0, 0.0, -1.0], [1.0, 0.0, 0.0]] k = 100.0 c = 30 # set parameters above translatingBody.setMass(mass) translatingBody.setK(k) translatingBody.setC(c) translatingBody.setRhoInit(rhoInit) translatingBody.setRhoDotInit(rhoDotInit) translatingBody.setFHat_B(fHat_B) translatingBody.setR_FcF_F(r_FcF_F) translatingBody.setR_F0B_B(r_F0B_B) translatingBody.setIPntFc_F(IPntFc_F) translatingBody.setDCM_FB(dcm_FB) translatingBody.ModelTag = "translatingBody" # Add translating body to spacecraft scObject.addStateEffector(translatingBody) # Create the reference message translationRef = messaging.LinearTranslationRigidBodyMsgPayload() translationRef.rho = rhoRef translationRef.rhoDot = 0.0 translationRefMsg = messaging.LinearTranslationRigidBodyMsg().write(translationRef) translatingBody.translatingBodyRefInMsg.subscribeTo(translationRefMsg) # Add test module to runtime call list unitTestSim.AddModelToTask(unitTaskName, translatingBody) unitTestSim.AddModelToTask(unitTaskName, scObject) # Add Earth gravity to the simulation earthGravBody = gravityEffector.GravBodyData() earthGravBody.planetName = "earth_planet_data" earthGravBody.mu = 0.3986004415E+15 # meters! earthGravBody.isCentralBody = True scObject.gravField.gravBodies = spacecraft.GravBodyVector([earthGravBody]) # Log the spacecraft state message datLog = scObject.scStateOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, datLog) # Add energy and momentum variables to log scObjectLog = scObject.logger(["totOrbAngMomPntN_N", "totRotAngMomPntC_N", "totOrbEnergy"]) unitTestSim.AddModelToTask(unitTaskName, scObjectLog) # Initialize the simulation unitTestSim.InitializeSimulation() # Add states to log rhoData = translatingBody.translatingBodyOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, rhoData) # Setup and run the simulation stopTime = 25000 * testProcessRate unitTestSim.ConfigureStopTime(stopTime) unitTestSim.ExecuteSimulation() # Extract the logged variables orbAngMom_N = scObjectLog.totOrbAngMomPntN_N rotAngMom_N = scObjectLog.totRotAngMomPntC_N orbEnergy = scObjectLog.totOrbEnergy rho = rhoData.rho rhoDot = rhoData.rhoDot # Set up the conservation quantities timeSec = scObjectLog.times() * 1e-9 initialOrbAngMom_N = [orbAngMom_N[0, 0], orbAngMom_N[0, 1], orbAngMom_N[0, 2]] finalOrbAngMom = orbAngMom_N[-1] initialRotAngMom_N = [rotAngMom_N[0, 0], rotAngMom_N[0, 1], rotAngMom_N[0, 2]] finalRotAngMom = rotAngMom_N[-1] initialOrbEnergy = orbEnergy[0] finalOrbEnergy = orbEnergy[-1] finalAngle = rho[-1] # Plotting plt.close("all") plt.figure() plt.clf() plt.plot(timeSec, (orbAngMom_N[:, 0] - initialOrbAngMom_N[0]) / initialOrbAngMom_N[0], timeSec, (orbAngMom_N[:, 1] - initialOrbAngMom_N[1]) / initialOrbAngMom_N[1], timeSec, (orbAngMom_N[:, 2] - initialOrbAngMom_N[2]) / initialOrbAngMom_N[2]) plt.xlabel('time (s)') plt.ylabel('Relative Difference') plt.title('Orbital Angular Momentum') plt.figure() plt.clf() plt.plot(timeSec, (orbEnergy - initialOrbEnergy) / initialOrbEnergy) plt.xlabel('time (s)') plt.ylabel('Relative Difference') plt.title('Orbital Energy') plt.figure() plt.clf() plt.plot(timeSec, (rotAngMom_N[:, 0] - initialRotAngMom_N[0]) / initialRotAngMom_N[0], timeSec, (rotAngMom_N[:, 1] - initialRotAngMom_N[1]) / initialRotAngMom_N[1], timeSec, (rotAngMom_N[:, 2] - initialRotAngMom_N[2]) / initialRotAngMom_N[2]) plt.xlabel('time (s)') plt.ylabel('Relative Difference') plt.title('Rotational Angular Momentum') plt.figure() plt.clf() plt.plot(timeSec, rho) plt.xlabel('time (s)') plt.ylabel('rho') plt.figure() plt.clf() plt.plot(timeSec, rhoDot) plt.xlabel('time (s)') plt.ylabel('rhoDot') if show_plots: plt.show() plt.close("all") # Testing setup accuracy = 1e-12 np.testing.assert_allclose(finalAngle, rhoRef, atol=0.01, err_msg="Angle doesn't settle to reference angle.") np.testing.assert_allclose(finalOrbEnergy, initialOrbEnergy, rtol=accuracy, err_msg="Orbital energy is not constant.") for i in range(3): np.testing.assert_allclose(finalOrbAngMom, initialOrbAngMom_N, rtol=accuracy, err_msg="Orbital angular momentum is not constant.") np.testing.assert_allclose(finalRotAngMom, initialRotAngMom_N, rtol=accuracy, err_msg="Rotational angular momentum is not constant.") if __name__ == "__main__": translatingBodyNoInput(True) # translatingBodyLockFlag(True) # translatingBodyCommandedForce(True, 1) # translatingBodyRhoReference(True, 0.5)