#
# ISC License
#
# Copyright (c) 2021, Autonomous Vehicle Systems Lab, University of Colorado 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 numpy as np
from Basilisk.architecture import messaging
from Basilisk.fswAlgorithms import smallBodyWaypointFeedback
from Basilisk.utilities import SimulationBaseClass
from Basilisk.utilities import macros
from Basilisk.utilities import orbitalMotion
from Basilisk.utilities import unitTestSupport
# @pytest.mark.parametrize("accuracy", [1e-12])
# @pytest.mark.parametrize("param1, param2", [
# (1, 1)
# ,(1, 3)
# ])
[docs]
def test_smallBodyWaypointFeedback(show_plots):
r"""
**Validation Test Description**
This test checks two things: a large force output when the spacecraft is far from the waypoint, and a small force
output when the spacecraft is at the waypoint.
**Test Parameters**
Args:
:param show_plots: flag if plots should be shown.
**Description of Variables Being Tested**
In this test, the ``forceRequestBody`` variable in the :ref:`CmdForceBodyMsgPayload` output by the module is tested.
When far away from the waypoint, the force request should be larger than 1 N. When close to the waypoint, the force
request should only account for third body perturbations and SRP.
"""
[testResults1, testMessages1] = smallBodyWaypointFeedbackTestFunction1()
[testResults2, testMessages2] = smallBodyWaypointFeedbackTestFunction2()
assert (testResults1 + testResults2) < 1, [testMessages1, testMessages2]
[docs]
def smallBodyWaypointFeedbackTestFunction1():
"""This test checks for a large force return when far away from the waypoint"""
testFailCount = 0
testMessages = []
unitTaskName = "unitTask"
unitProcessName = "TestProcess"
unitTestSim = SimulationBaseClass.SimBaseClass()
testProcessRate = macros.sec2nano(0.5)
testProc = unitTestSim.CreateNewProcess(unitProcessName)
testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate))
# setup module to be tested
module = smallBodyWaypointFeedback.SmallBodyWaypointFeedback()
module.ModelTag = "smallBodyWaypointFeedback1"
unitTestSim.AddModelToTask(unitTaskName, module)
module.A_sc = 1. # Surface area of the spacecraft, m^2
module.M_sc = 300 # Mass of the spacecraft, kg
module.IHubPntC_B = unitTestSupport.np2EigenMatrix3d([82.12, 0.0, 0.0, 0.0, 98.40, 0.0, 0.0, 0.0, 121.0]) # sc inertia
module.mu_ast = 4.892 # Gravitational constant of the asteroid
module.x1_ref = [-2000., 0., 0.]
module.x2_ref = [0.0, 0.0, 0.0]
module.K1 = unitTestSupport.np2EigenMatrix3d([5e-4, 0e-5, 0e-5, 0e-5, 5e-4, 0e-5, 0e-5, 0e-5, 5e-4])
module.K2 = unitTestSupport.np2EigenMatrix3d([1., 0., 0., 0., 1., 0., 0., 0., 1.])
# Set the orbital parameters of the asteroid
oeAsteroid = orbitalMotion.ClassicElements()
oeAsteroid.a = 1.1259 * orbitalMotion.AU * 1000 # meters
oeAsteroid.e = 0.20373
oeAsteroid.i = 6.0343 * macros.D2R
oeAsteroid.Omega = 2.01820 * macros.D2R
oeAsteroid.omega = 66.304 * macros.D2R
oeAsteroid.f = 346.32 * macros.D2R
r_ON_N, v_ON_N = orbitalMotion.elem2rv(orbitalMotion.MU_SUN*(1000.**3), oeAsteroid)
# Create the position and velocity of states of the s/c wrt the small body hill frame origin
r_BO_N = np.array([-2000., 1500., 1000.]) # Position of the spacecraft relative to the body
v_BO_N = np.array([0., 0., 0.]) # Velocity of the spacecraft relative to the body
# Create the inertial position and velocity of the s/c
r_BN_N = np.add(r_BO_N, r_ON_N)
v_BN_N = np.add(v_BO_N, v_ON_N)
# Configure blank module input messages
asteroidEphemerisInMsgData = messaging.EphemerisMsgPayload()
asteroidEphemerisInMsgData.r_BdyZero_N = r_ON_N
asteroidEphemerisInMsgData.v_BdyZero_N = v_ON_N
asteroidEphemerisInMsg = messaging.EphemerisMsg().write(asteroidEphemerisInMsgData)
navTransInMsgData = messaging.NavTransMsgPayload()
navTransInMsgData.r_BN_N = r_BN_N
navTransInMsgData.v_BN_N = v_BN_N
navTransInMsg = messaging.NavTransMsg().write(navTransInMsgData)
navAttInMsgData = messaging.NavAttMsgPayload()
navAttInMsgData.sigma_BN = np.array([0.1, 0.0, 0.0])
navAttInMsg = messaging.NavAttMsg().write(navAttInMsgData)
sunEphemerisInMsgData = messaging.EphemerisMsgPayload()
sunEphemerisInMsg = messaging.EphemerisMsg().write(sunEphemerisInMsgData)
# subscribe input messages to module
module.navTransInMsg.subscribeTo(navTransInMsg)
module.navAttInMsg.subscribeTo(navAttInMsg)
module.asteroidEphemerisInMsg.subscribeTo(asteroidEphemerisInMsg)
module.sunEphemerisInMsg.subscribeTo(sunEphemerisInMsg)
# setup output message recorder objects
forceOutMsgRec = module.forceOutMsg.recorder()
unitTestSim.AddModelToTask(unitTaskName, forceOutMsgRec)
unitTestSim.InitializeSimulation()
unitTestSim.ConfigureStopTime(macros.sec2nano(0.))
unitTestSim.ExecuteSimulation()
if np.linalg.norm(forceOutMsgRec.forceRequestBody) <= 1:
testFailCount += 1
testMessages.append("FAILED: " + module.ModelTag + " Module failed "
+ "force output" + " unit test")
if testFailCount == 0:
print("PASSED: " + module.ModelTag)
else:
print(testMessages)
return [testFailCount, "".join(testMessages)]
[docs]
def smallBodyWaypointFeedbackTestFunction2():
"""This test checks that the force output is near zero when at the waypoint"""
testFailCount = 0
testMessages = []
unitTaskName = "unitTask"
unitProcessName = "TestProcess"
unitTestSim = SimulationBaseClass.SimBaseClass()
testProcessRate = macros.sec2nano(0.5)
testProc = unitTestSim.CreateNewProcess(unitProcessName)
testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate))
# setup module to be tested
module = smallBodyWaypointFeedback.SmallBodyWaypointFeedback()
module.ModelTag = "smallBodyWaypointFeedback2"
unitTestSim.AddModelToTask(unitTaskName, module)
module.A_sc = 1. # Surface area of the spacecraft, m^2
module.M_sc = 300 # Mass of the spacecraft, kg
module.IHubPntC_B = unitTestSupport.np2EigenMatrix3d([82.12, 0.0, 0.0, 0.0, 98.40, 0.0, 0.0, 0.0, 121.0]) # sc inertia
module.mu_ast = 4.892 # Gravitational constant of the asteroid
module.x1_ref = [-2000., 0., 0.]
module.x2_ref = [0.0, 0.0, 0.0]
module.K1 = unitTestSupport.np2EigenMatrix3d([5e-4, 0e-5, 0e-5, 0e-5, 5e-4, 0e-5, 0e-5, 0e-5, 5e-4])
module.K2 = unitTestSupport.np2EigenMatrix3d([1., 0., 0., 0., 1., 0., 0., 0., 1.])
# Set the orbital parameters of the asteroid
oeAsteroid = orbitalMotion.ClassicElements()
oeAsteroid.a = 1.1259 * orbitalMotion.AU * 1000 # meters
oeAsteroid.e = 0.20373
oeAsteroid.i = 6.0343 * macros.D2R
oeAsteroid.Omega = 2.01820 * macros.D2R
oeAsteroid.omega = 66.304 * macros.D2R
oeAsteroid.f = 346.32 * macros.D2R
r_ON_N, v_ON_N = orbitalMotion.elem2rv(orbitalMotion.MU_SUN*(1000.**3), oeAsteroid)
# Create the position and velocity of states of the s/c wrt the small body hill frame
r_BO_H = np.array([-2000., 0., 0.]) # Position of the spacecraft relative to the body
v_BO_H = np.array([0., 0., 0.]) # Velocity of the spacecraft relative to the body
r_BN_N, v_BN_N = orbitalMotion.hill2rv(r_ON_N, v_ON_N, r_BO_H, v_BO_H)
# Configure blank module input messages
asteroidEphemerisInMsgData = messaging.EphemerisMsgPayload()
asteroidEphemerisInMsgData.r_BdyZero_N = r_ON_N
asteroidEphemerisInMsgData.v_BdyZero_N = v_ON_N
asteroidEphemerisInMsg = messaging.EphemerisMsg().write(asteroidEphemerisInMsgData)
navTransInMsgData = messaging.NavTransMsgPayload()
navTransInMsgData.r_BN_N = r_BN_N
navTransInMsgData.v_BN_N = v_BN_N
navTransInMsg = messaging.NavTransMsg().write(navTransInMsgData)
navAttInMsgData = messaging.NavAttMsgPayload()
navAttInMsgData.sigma_BN = np.array([0.1, 0.0, 0.0])
navAttInMsg = messaging.NavAttMsg().write(navAttInMsgData)
sunEphemerisInMsgData = messaging.EphemerisMsgPayload()
sunEphemerisInMsg = messaging.EphemerisMsg().write(sunEphemerisInMsgData)
# subscribe input messages to module
module.navTransInMsg.subscribeTo(navTransInMsg)
module.navAttInMsg.subscribeTo(navAttInMsg)
module.asteroidEphemerisInMsg.subscribeTo(asteroidEphemerisInMsg)
module.sunEphemerisInMsg.subscribeTo(sunEphemerisInMsg)
# setup output message recorder objects
forceOutMsgRec = module.forceOutMsg.recorder()
unitTestSim.AddModelToTask(unitTaskName, forceOutMsgRec)
unitTestSim.InitializeSimulation()
unitTestSim.ConfigureStopTime(macros.sec2nano(0.))
unitTestSim.ExecuteSimulation()
if np.linalg.norm(forceOutMsgRec.forceRequestBody) >= 1e-8:
testFailCount += 1
testMessages.append("FAILED: " + module.ModelTag + " Module failed "
+ "force output" + " unit test")
if testFailCount == 0:
print("PASSED: " + module.ModelTag)
else:
print(testMessages)
return [testFailCount, "".join(testMessages)]
if __name__ == "__main__":
test_smallBodyWaypointFeedback(False)