#
# ISC License
#
# Copyright (c) 2021, 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.
#
r"""
Overview
--------
This scenario demonstrates how to use the ``smallBodyWaypointFeedback()`` module for waypoint to waypoint motion about a
small body. In this scenario, the spacecraft holds an inertial pointing attitude while it moves to a waypoint defined in the small
body's Hill frame. The :ref:`simpleNav` and :ref:`planetNav` moduels provide measurements to the control law in the form
of :ref:`navTransMsgPayload`, :ref:`navAttMsgPayload`, and :ref:`ephemerisMsgPayload` input messages. The control law
outputs a :ref:`CmdForceBodyMsgPayload`, which is read in by :ref:`extForceTorque`.
The control output in the spacecraft body frame can be found in the following plot:
.. image:: /_images/Scenarios/scenarioSmallBodyFeedbackControl3.svg
:align: center
The difference between the spacecraft position and velocity and associated references may be found in the figures below:
.. image:: /_images/Scenarios/scenarioSmallBodyFeedbackControl1.svg
:align: center
.. image:: /_images/Scenarios/scenarioSmallBodyFeedbackControl2.svg
:align: center
Finally, the attitude and attitude rate is given in the plots below.
.. image:: /_images/Scenarios/scenarioSmallBodyFeedbackControl4.svg
:align: center
.. image:: /_images/Scenarios/scenarioSmallBodyFeedbackControl5.svg
:align: center
The script is found in the folder ``basilisk/examples`` and executed by using::
python3 scenarioSmallBodyFeedbackControl.py
"""
#
# Basilisk Scenario Script and Integrated Test
#
# Purpose: Example simulation to demonstrate the use of the smallBodyWaypointFeedback module
# Author: Adam Herrmann
# Creation Date: March 28th, 2022
#
import math
import os
import matplotlib.pyplot as plt
import numpy as np
from Basilisk.architecture import messaging
from Basilisk.fswAlgorithms import attTrackingError
from Basilisk.fswAlgorithms import inertial3D
from Basilisk.fswAlgorithms import mrpFeedback
from Basilisk.fswAlgorithms import rwMotorTorque
from Basilisk.fswAlgorithms import smallBodyWaypointFeedback
from Basilisk.simulation import ephemerisConverter
from Basilisk.simulation import extForceTorque
from Basilisk.simulation import planetEphemeris
from Basilisk.simulation import planetNav
from Basilisk.simulation import radiationPressure
from Basilisk.simulation import reactionWheelStateEffector
from Basilisk.simulation import simpleNav
from Basilisk.simulation import spacecraft
from Basilisk.utilities import (SimulationBaseClass, macros, simIncludeGravBody, vizSupport)
from Basilisk.utilities import orbitalMotion
from Basilisk.utilities import simIncludeRW
from Basilisk.utilities import unitTestSupport
try:
from Basilisk.simulation import vizInterface
vizFound = True
except ImportError:
vizFound = False
# The path to the location of Basilisk
# Used to get the location of supporting data.
fileName = os.path.basename(os.path.splitext(__file__)[0])
# Plotting functions
[docs]def plot_position(time, r_BO_O_truth, r_BO_O_meas):
"""Plot the relative position result."""
fig, ax = plt.subplots(3, sharex=True, figsize=(12,6))
fig.add_subplot(111, frameon=False)
plt.tick_params(labelcolor='none', top=False, bottom=False, left=False, right=False)
ax[0].plot(time, r_BO_O_meas[:, 0], 'k*', label='measurement', markersize=1)
ax[1].plot(time, r_BO_O_meas[:, 1], 'k*', markersize=1)
ax[2].plot(time, r_BO_O_meas[:, 2], 'k*', markersize=1)
ax[0].plot(time, r_BO_O_truth[:, 0], label='${}^Or_{BO_{1}}$')
ax[1].plot(time, r_BO_O_truth[:, 1], label='${}^Or_{BO_{2}}$')
ax[2].plot(time, r_BO_O_truth[:, 2], label='${}^Or_{BO_{3}}$')
plt.xlabel('Time [sec]')
plt.title('Relative Spacecraft Position')
ax[0].set_ylabel('${}^Or_{BO_1}$ [m]')
ax[1].set_ylabel('${}^Or_{BO_2}$ [m]')
ax[2].set_ylabel('${}^Or_{BO_3}$ [m]')
ax[0].legend()
return
[docs]def plot_velocity(time, v_BO_O_truth, v_BO_O_meas):
"""Plot the relative velocity result."""
plt.gcf()
fig, ax = plt.subplots(3, sharex=True, figsize=(12,6))
fig.add_subplot(111, frameon=False)
plt.tick_params(labelcolor='none', top=False, bottom=False, left=False, right=False)
ax[0].plot(time, v_BO_O_meas[:, 0], 'k*', label='measurement', markersize=1)
ax[1].plot(time, v_BO_O_meas[:, 1], 'k*', markersize=1)
ax[2].plot(time, v_BO_O_meas[:, 2], 'k*', markersize=1)
ax[0].plot(time, v_BO_O_truth[:, 0], label='truth')
ax[1].plot(time, v_BO_O_truth[:, 1])
ax[2].plot(time, v_BO_O_truth[:, 2])
plt.xlabel('Time [sec]')
plt.title('Relative Spacecraft Velocity')
ax[0].set_ylabel('${}^Ov_{BO_1}$ [m/s]')
ax[1].set_ylabel('${}^Ov_{BO_2}$ [m/s]')
ax[2].set_ylabel('${}^Ov_{BO_3}$ [m/s]')
ax[0].legend()
return
def plot_sc_att(time, sigma_BN_truth, sigma_BN_meas):
plt.gcf()
fig, ax = plt.subplots(3, sharex=True, sharey=True, figsize=(12,6))
fig.add_subplot(111, frameon=False)
plt.tick_params(labelcolor='none', top=False, bottom=False, left=False, right=False)
ax[0].plot(time, sigma_BN_meas[:, 0], 'k*', label='measurement', markersize=1)
ax[1].plot(time, sigma_BN_meas[:, 1], 'k*', markersize=1)
ax[2].plot(time, sigma_BN_meas[:, 2], 'k*', markersize=1)
ax[0].plot(time, sigma_BN_truth[:, 0], label='truth')
ax[1].plot(time, sigma_BN_truth[:, 1])
ax[2].plot(time, sigma_BN_truth[:, 2])
plt.xlabel('Time [sec]')
ax[0].set_ylabel(r'$\sigma_{BN_1}$ [rad]')
ax[1].set_ylabel(r'$\sigma_{BN_2}$ [rad]')
ax[2].set_ylabel(r'$\sigma_{BN_3}$ [rad]')
ax[0].legend()
return
def plot_sc_rate(time, omega_BN_B_truth, omega_BN_B_meas):
plt.gcf()
fig, ax = plt.subplots(3, sharex=True, sharey=True, figsize=(12,6))
fig.add_subplot(111, frameon=False)
plt.tick_params(labelcolor='none', top=False, bottom=False, left=False, right=False)
ax[0].plot(time, omega_BN_B_meas[:, 0], 'k*', label='measurement', markersize=1)
ax[1].plot(time, omega_BN_B_meas[:, 1], 'k*', markersize=1)
ax[2].plot(time, omega_BN_B_meas[:, 2], 'k*', markersize=1)
ax[0].plot(time, omega_BN_B_truth[:, 0], label='truth')
ax[1].plot(time, omega_BN_B_truth[:, 1])
ax[2].plot(time, omega_BN_B_truth[:, 2])
plt.xlabel('Time [sec]')
ax[0].set_ylabel(r'${}^B\omega_{BN_{1}}$ [rad/s]')
ax[1].set_ylabel(r'${}^B\omega_{BN_{2}}$ [rad/s]')
ax[2].set_ylabel(r'${}^B\omega_{BN_{3}}$ [rad/s]')
ax[0].legend()
return
def plot_control(time, u):
plt.gcf()
fig, ax = plt.subplots(3, sharex=True, sharey=True, figsize=(12,6))
fig.add_subplot(111, frameon=False)
plt.tick_params(labelcolor='none', top=False, bottom=False, left=False, right=False)
ax[0].plot(time, u[:, 0], 'k-', markersize=1)
ax[1].plot(time, u[:, 1], 'k-', markersize=1)
ax[2].plot(time, u[:, 2], 'k-', markersize=1)
plt.xlabel('Time [sec]')
ax[0].set_ylabel(r'$\hat{\mathbf{b}}_1$ control [N]')
ax[1].set_ylabel(r'$\hat{\mathbf{b}}_2$ control [N]')
ax[2].set_ylabel(r'$\hat{\mathbf{b}}_3$ control [N]')
return
[docs]def run(show_plots):
"""
The scenarios can be run with the followings setups parameters:
Args:
show_plots (bool): Determines if the script should display plots
"""
path = os.path.dirname(os.path.abspath(__file__))
# Create simulation variable names
simTaskName = "simTask"
simProcessName = "simProcess"
# Create a sim module as an empty container
scSim = SimulationBaseClass.SimBaseClass()
# Create the simulation process
dynProcess = scSim.CreateNewProcess(simProcessName)
# Create the dynamics task and specify the simulation time step information
simulationTimeStep = macros.sec2nano(1.0)
dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep))
# Setup celestial object ephemeris module
gravBodyEphem = planetEphemeris.PlanetEphemeris()
gravBodyEphem.ModelTag = 'planetEphemeris'
gravBodyEphem.setPlanetNames(planetEphemeris.StringVector(["bennu"]))
# specify orbits of gravitational bodies
# https://ssd.jpl.nasa.gov/horizons.cgi#results
# December 31st, 2018
oeAsteroid = planetEphemeris.ClassicElementsMsgPayload()
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)
# specify celestial object orbit
gravBodyEphem.planetElements = planetEphemeris.classicElementVector([oeAsteroid])
gravBodyEphem.rightAscension = planetEphemeris.DoubleVector([0. * macros.D2R])
gravBodyEphem.declination = planetEphemeris.DoubleVector([90. * macros.D2R])
gravBodyEphem.lst0 = planetEphemeris.DoubleVector([0.0 * macros.D2R])
gravBodyEphem.rotRate = planetEphemeris.DoubleVector([360 * macros.D2R / (4.297461 * 3600.)])
# setup Sun Gravity Body
gravFactory = simIncludeGravBody.gravBodyFactory()
gravFactory.createSun()
# Create a sun spice message, zero it out, required by srp
sunPlanetStateMsgData = messaging.SpicePlanetStateMsgPayload()
sunPlanetStateMsg = messaging.SpicePlanetStateMsg()
sunPlanetStateMsg.write(sunPlanetStateMsgData)
# Create a sun ephemeris message, zero it out, required by nav filter
sunEphemerisMsgData = messaging.EphemerisMsgPayload()
sunEphemerisMsg = messaging.EphemerisMsg()
sunEphemerisMsg.write(sunEphemerisMsgData)
mu = 4.892 # m^3/s^2
asteroid = gravFactory.createCustomGravObject("bennu", mu)
asteroid.planetBodyInMsg.subscribeTo(gravBodyEphem.planetOutMsgs[0])
# create SC object
scObject = spacecraft.Spacecraft()
scObject.ModelTag = "bskSat"
gravFactory.addBodiesTo(scObject)
# 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)
# Set the truth ICs for the spacecraft position and velocity
scObject.hub.r_CN_NInit = r_BN_N # m - r_BN_N
scObject.hub.v_CN_NInit = v_BN_N # m/s - v_BN_N
I = [82.12, 0.0, 0.0, 0.0, 98.40, 0.0, 0.0, 0.0, 121.0]
mass = 330. # kg
scObject.hub.mHub = mass
scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I)
# Set the truth ICs for the spacecraft attitude and rate
scObject.hub.sigma_BNInit = np.array([0.1, 0.0, 0.0]) # rad
scObject.hub.omega_BN_BInit = np.array([0.1, 0.1, 0.1]) # rad/s
# Create RWs
rwFactory = simIncludeRW.rwFactory()
# create each RW by specifying the RW type, the spin axis gsHat, plus optional arguments
RW1 = rwFactory.create('Honeywell_HR16', [1, 0, 0], maxMomentum=100., Omega=100. # RPM
)
RW2 = rwFactory.create('Honeywell_HR16', [0, 1, 0], maxMomentum=100., Omega=200. # RPM
)
RW3 = rwFactory.create('Honeywell_HR16', [0, 0, 1], maxMomentum=100., Omega=300. # RPM
)
# create RW object container and tie to spacecraft object
rwStateEffector = reactionWheelStateEffector.ReactionWheelStateEffector()
rwStateEffector.ModelTag = "RW_cluster"
rwFactory.addToSpacecraft(scObject.ModelTag, rwStateEffector, scObject)
rwConfigMsg = rwFactory.getConfigMessage()
# Create an SRP model
srp = radiationPressure.RadiationPressure() # default model is the SRP_CANNONBALL_MODEL
srp.area = 1. # m^3
srp.coefficientReflection = 1.9
scObject.addDynamicEffector(srp)
srp.sunEphmInMsg.subscribeTo(sunPlanetStateMsg)
# Create an ephemeris converter
ephemConverter = ephemerisConverter.EphemerisConverter()
ephemConverter.ModelTag = "ephemConverter"
ephemConverter.addSpiceInputMsg(gravBodyEphem.planetOutMsgs[0])
# Set up simpleNav for s/c "measurements"
simpleNavMeas = simpleNav.SimpleNav()
simpleNavMeas.ModelTag = 'SimpleNav'
simpleNavMeas.scStateInMsg.subscribeTo(scObject.scStateOutMsg)
pos_sigma_sc = 30.0
vel_sigma_sc = 0.01
att_sigma_sc = 0.1 * math.pi / 180.0
rate_sigma_sc = 0.05 * math.pi / 180.0
sun_sigma_sc = 0.0
dv_sigma_sc = 0.0
p_matrix_sc = [[pos_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., pos_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., pos_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., vel_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., vel_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., vel_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., att_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., att_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., att_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., rate_sigma_sc, 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., rate_sigma_sc, 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., rate_sigma_sc, 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., sun_sigma_sc, 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., sun_sigma_sc, 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., sun_sigma_sc, 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., dv_sigma_sc, 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., dv_sigma_sc, 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., dv_sigma_sc]]
walk_bounds_sc = [[10.], [10.], [10.], [0.001], [0.001], [0.001], [0.005], [0.005], [0.005], [0.002], [0.002], [0.002], [0.], [0.], [0.], [0.], [0.], [0.]]
simpleNavMeas.PMatrix = p_matrix_sc
simpleNavMeas.walkBounds = walk_bounds_sc
# Set up planetNav for Bennu "measurements"
planetNavMeas = planetNav.PlanetNav()
planetNavMeas.ephemerisInMsg.subscribeTo(ephemConverter.ephemOutMsgs[0])
# Define the Pmatrix for planetNav, no uncertainty on position and velocity of the body
pos_sigma_p = 0.0
vel_sigma_p = 0.0
att_sigma_p = 2.0 * math.pi / 180.0
rate_sigma_p = 0.3 * math.pi / 180.0
p_matrix_p = [[pos_sigma_p, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., pos_sigma_p, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., pos_sigma_p, 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., vel_sigma_p, 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., vel_sigma_p, 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., vel_sigma_p, 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., att_sigma_p, 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., att_sigma_p, 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., att_sigma_p, 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., rate_sigma_p, 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., rate_sigma_p, 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., rate_sigma_p]]
walk_bounds_p = [[0.], [0.], [0.], [0.], [0.], [0.], [0.005], [0.005], [0.005], [0.002], [0.002], [0.002]]
planetNavMeas.PMatrix = p_matrix_p
planetNavMeas.walkBounds = walk_bounds_p
# Inertial pointing
inertialPoint = inertial3D.inertial3D()
inertialPoint.ModelTag = "inertialPoint"
inertialPoint.sigma_R0N = [0.1, 0.0, 0.0]
# Attitude error configuration
trackingError = attTrackingError.attTrackingError()
trackingError.ModelTag = "trackingError"
trackingError.attRefInMsg.subscribeTo(inertialPoint.attRefOutMsg)
# Specify the vehicle configuration message to tell things what the vehicle inertia is
vehicleConfigOut = messaging.VehicleConfigMsgPayload()
vehicleConfigOut.ISCPntB_B = I
vehicleConfigOut.CoM_B = [0.0, 0.0, 0.0]
vcConfigMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut)
# Attitude controller configuration
mrpFeedbackControl = mrpFeedback.mrpFeedback()
mrpFeedbackControl.ModelTag = "mrpFeedbackControl"
mrpFeedbackControl.guidInMsg.subscribeTo(trackingError.attGuidOutMsg)
mrpFeedbackControl.vehConfigInMsg.subscribeTo(vcConfigMsg)
mrpFeedbackControl.K = 7.0
mrpFeedbackControl.Ki = -1
mrpFeedbackControl.P = 30.
mrpFeedbackControl.integralLimit = 2. / mrpFeedbackControl.Ki * 0.1
# add module that maps the Lr control torque into the RW motor torques
rwMotorTorqueObj = rwMotorTorque.rwMotorTorque()
rwMotorTorqueObj.ModelTag = "rwMotorTorque"
rwStateEffector.rwMotorCmdInMsg.subscribeTo(rwMotorTorqueObj.rwMotorTorqueOutMsg)
rwMotorTorqueObj.rwParamsInMsg.subscribeTo(rwConfigMsg)
rwMotorTorqueObj.vehControlInMsg.subscribeTo(mrpFeedbackControl.cmdTorqueOutMsg)
rwMotorTorqueObj.controlAxes_B = [1, 0, 0, 0, 1, 0, 0, 0, 1]
rwStateEffector.rwMotorCmdInMsg.subscribeTo(rwMotorTorqueObj.rwMotorTorqueOutMsg)
# Connect the smallBodyEKF output messages to the relevant modules
trackingError.attNavInMsg.subscribeTo(simpleNavMeas.attOutMsg)
# Create the Lyapunov feedback controller
waypointFeedback = smallBodyWaypointFeedback.SmallBodyWaypointFeedback()
waypointFeedback.asteroidEphemerisInMsg.subscribeTo(planetNavMeas.ephemerisOutMsg)
waypointFeedback.sunEphemerisInMsg.subscribeTo(sunEphemerisMsg)
waypointFeedback.navAttInMsg.subscribeTo(simpleNavMeas.attOutMsg)
waypointFeedback.navTransInMsg.subscribeTo(simpleNavMeas.transOutMsg)
waypointFeedback.A_sc = 1. # Surface area of the spacecraft, m^2
waypointFeedback.M_sc = mass # Mass of the spacecraft, kg
waypointFeedback.IHubPntC_B = unitTestSupport.np2EigenMatrix3d(I) # sc inertia
waypointFeedback.mu_ast = mu # Gravitational constant of the asteroid
waypointFeedback.x1_ref = [-2000., 0., 0.]
waypointFeedback.x2_ref = [0.0, 0.0, 0.0]
extForceTorqueModule = extForceTorque.ExtForceTorque()
extForceTorqueModule.cmdForceBodyInMsg.subscribeTo(waypointFeedback.forceOutMsg)
scObject.addDynamicEffector(extForceTorqueModule)
scSim.AddModelToTask(simTaskName, scObject, 200)
scSim.AddModelToTask(simTaskName, srp, 199)
scSim.AddModelToTask(simTaskName, gravBodyEphem, 198)
scSim.AddModelToTask(simTaskName, rwStateEffector, 197)
scSim.AddModelToTask(simTaskName, ephemConverter, 197)
scSim.AddModelToTask(simTaskName, simpleNavMeas, 196)
scSim.AddModelToTask(simTaskName, planetNavMeas, 195)
scSim.AddModelToTask(simTaskName, inertialPoint, 108)
scSim.AddModelToTask(simTaskName, trackingError, 106)
scSim.AddModelToTask(simTaskName, mrpFeedbackControl, 105)
scSim.AddModelToTask(simTaskName, extForceTorqueModule, 82)
scSim.AddModelToTask(simTaskName, rwMotorTorqueObj, 81)
scSim.AddModelToTask(simTaskName, waypointFeedback, 78)
# Setup data logging before the simulation is initialized
sc_truth_recorder = scObject.scStateOutMsg.recorder()
ast_truth_recorder = gravBodyEphem.planetOutMsgs[0].recorder()
ast_ephemeris_recorder = ephemConverter.ephemOutMsgs[0].recorder()
ast_ephemeris_meas_recorder = planetNavMeas.ephemerisOutMsg.recorder()
sc_meas_recorder = simpleNavMeas.transOutMsg.recorder()
sc_att_meas_recorder = simpleNavMeas.attOutMsg.recorder()
requested_control_recorder = waypointFeedback.forceOutMsg.recorder()
attitude_error_recorder = trackingError.attGuidOutMsg.recorder()
scSim.AddModelToTask(simTaskName, sc_truth_recorder)
scSim.AddModelToTask(simTaskName, ast_truth_recorder)
scSim.AddModelToTask(simTaskName, sc_meas_recorder)
scSim.AddModelToTask(simTaskName, sc_att_meas_recorder)
scSim.AddModelToTask(simTaskName, ast_ephemeris_recorder)
scSim.AddModelToTask(simTaskName, ast_ephemeris_meas_recorder)
scSim.AddModelToTask(simTaskName, requested_control_recorder)
scSim.AddModelToTask(simTaskName, attitude_error_recorder)
fileName = 'scenarioSmallBodyFeedbackControl'
if vizSupport.vizFound:
vizInterface = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject
# , saveFile=fileName
)
vizSupport.createStandardCamera(vizInterface, setMode=0, bodyTarget='bennu', setView=0)
# vizInterface.settings.showSpacecraftLabels = 1
vizInterface.settings.showCSLabels = 1
vizInterface.settings.planetCSon = 1
vizInterface.settings.orbitLinesOn = -1
# initialize Simulation
scSim.InitializeSimulation()
simulationTime_1 = macros.sec2nano(15000.0)
waypointFeedback.K1 = unitTestSupport.np2EigenMatrix3d([5e-4, 0e-5, 0e-5, 0e-5, 5e-4, 0e-5, 0e-5, 0e-5, 5e-4])
waypointFeedback.K2 = unitTestSupport.np2EigenMatrix3d([1., 0., 0., 0., 1., 0., 0., 0., 1.])
# configure a simulation stop time and execute the simulation run
scSim.ConfigureStopTime(simulationTime_1)
scSim.ExecuteSimulation()
# retrieve logged spacecraft position relative to asteroid
r_BN_N_truth = sc_truth_recorder.r_BN_N
r_BN_N_meas = sc_meas_recorder.r_BN_N
v_BN_N_truth = sc_truth_recorder.v_BN_N
v_BN_N_meas = sc_meas_recorder.v_BN_N
sigma_BN_truth = sc_truth_recorder.sigma_BN
sigma_BN_meas = sc_att_meas_recorder.sigma_BN
omega_BN_B_truth = sc_truth_recorder.omega_BN_B
omega_BN_B_meas = sc_att_meas_recorder.omega_BN_B
r_AN_N = ast_truth_recorder.PositionVector
v_AN_N = ast_truth_recorder.VelocityVector
u_requested = requested_control_recorder.forceRequestBody
# Compute the relative position and velocity of the s/c in the small body hill frame
r_BO_O_truth = []
v_BO_O_truth = []
r_BO_O_meas = []
v_BO_O_meas = []
np.set_printoptions(precision=15)
for rd_N, vd_N, rc_N, vc_N, rd_N_meas, vd_N_meas in zip(r_BN_N_truth, v_BN_N_truth, r_AN_N, v_AN_N, r_BN_N_meas, v_BN_N_meas):
# Truth values
r_BO_O, v_BO_O = orbitalMotion.rv2hill(rc_N, vc_N, rd_N, vd_N)
r_BO_O_truth.append(r_BO_O)
v_BO_O_truth.append(v_BO_O)
# Measurement values
r_BO_O, v_BO_O = orbitalMotion.rv2hill(rc_N, vc_N, rd_N_meas, vd_N_meas)
r_BO_O_meas.append(r_BO_O)
v_BO_O_meas.append(v_BO_O)
# print(rd_N)
# print(vd_N)
# Plot the results
time = sc_truth_recorder.times() * macros.NANO2SEC
plot_position(time, np.array(r_BO_O_truth), np.array(r_BO_O_meas))
figureList = {}
pltName = fileName + "1"
figureList[pltName] = plt.figure(1)
plot_velocity(time, np.array(v_BO_O_truth), np.array(v_BO_O_meas))
pltName = fileName + "2"
figureList[pltName] = plt.figure(2)
plot_control(time, np.array(u_requested))
pltName = fileName + "3"
figureList[pltName] = plt.figure(3)
plot_sc_att(time, np.array(sigma_BN_truth), np.array(sigma_BN_meas))
pltName = fileName + "4"
figureList[pltName] = plt.figure(4)
plot_sc_rate(time, np.array(omega_BN_B_truth), np.array(omega_BN_B_meas))
pltName = fileName + "5"
figureList[pltName] = plt.figure(5)
if show_plots:
plt.show()
# close the plots being saved off to avoid over-writing old and new figures
plt.close("all")
return figureList
#
# This statement below ensures that the unit test scrip can be run as a
# stand-along python script
#
if __name__ == "__main__":
run(
True # show_plots
)