#
# ISC License
#
# Copyright (c) 2016, 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
--------
Demonstrates a basic debris reorbit scenario from geostationary orbit using the Electrostatic Tractor (ET) concept and how to visualize the simulation
data in :ref:`Vizard <vizard>`. This scenario shows how to use the :ref:`etSphericalControl` module for ET relative
motion control and also illustrates the usage of the :ref:`msmForceTorque` to calculate the electrostatic forces with the Multi-Sphere Method (MSM). This simulation simply uses a single sphere to represent each spacecraft. The servicing satellite is charged to a positive electric potential, while the other satellite (the debris) is uncontrolled and charged to a negative potential. The purpose of this script is to show an explicit method to
setup the ET reorbit simulation, and also show how to store the Basilisk simulation data to be able to visualize
both satellite's motions within the :ref:`Vizard <vizard>` application.
The script is found in the folder ``src/examples`` and executed by using::
python3 scenarioDebrisReorbitET.py
The simulation layout is shown in the following illustration. A single simulation process is created
which contains both the servicer spacecraft and the associated Flight Software (FSW) algorithm
module, as well as the debris object.
.. image:: /_images/static/test_scenarioDebrisReorbitET.svg
:align: center
When the simulation completes several plots are shown for the separation distance of the two satellites, the inertial position of both spacecraft, and the semi-major axis change of the debris.
Illustration of Simulation Results
----------------------------------
::
show_plots = True
.. image:: /_images/Scenarios/scenarioDebrisReorbitET1.svg
:align: center
.. image:: /_images/Scenarios/scenarioDebrisReorbitET2.svg
:align: center
.. image:: /_images/Scenarios/scenarioDebrisReorbitET3.svg
:align: center
"""
#
# Basilisk Scenario Script and Integrated Test
#
# Purpose: Basic simulation showing a servicer that reorbits a debris using electrostatic forces.
# Author: Julian Hammerl
# Creation Date: May 20, 2021
#
import os
import matplotlib.pyplot as plt
import numpy as np
from Basilisk.architecture import messaging
from Basilisk.fswAlgorithms import etSphericalControl
from Basilisk.simulation import simpleNav, spacecraft, extForceTorque, msmForceTorque
from Basilisk.utilities import (SimulationBaseClass, macros,
orbitalMotion, simIncludeGravBody,
unitTestSupport, vizSupport)
try:
from Basilisk.simulation import vizInterface
except ImportError:
pass
# The path to the location of Basilisk
# Used to get the location of supporting data.
from Basilisk import __path__
bskPath = __path__[0]
fileName = os.path.basename(os.path.splitext(__file__)[0])
[docs]
def run(show_plots):
"""
The scenarios can be run with the followings setup parameters:
Args:
show_plots (bool): Determines if the script should display plots
"""
# Create simulation variable names
dynTaskName = "dynTask"
dynProcessName = "dynProcess"
# Create a sim module as an empty container
scSim = SimulationBaseClass.SimBaseClass()
#
# create the simulation process
#
dynProcess = scSim.CreateNewProcess(dynProcessName, 1)
# create the dynamics task and specify the integration update time
simulationTimeStep = macros.sec2nano(300.0)
dynProcess.addTask(scSim.CreateNewTask(dynTaskName, simulationTimeStep))
#
# setup the simulation tasks/objects
#
# initialize servicer spacecraft object and set properties
scObjectServicer = spacecraft.Spacecraft()
scObjectServicer.ModelTag = "Servicer"
scObjectServicer.hub.mHub = 500.0 # [kg] servicer mass
# initialize servicer spacecraft object and set properties
scObjectDebris = spacecraft.Spacecraft()
scObjectDebris.ModelTag = "DebrisSat"
scObjectDebris.hub.mHub = 2000.0 # kg
# add spacecraftPlus object to the simulation process
scSim.AddModelToTask(dynTaskName, scObjectServicer)
scSim.AddModelToTask(dynTaskName, scObjectDebris)
# Create VehicleConfig messages including the S/C mass (for etSphericalControl)
servicerConfigOutData = messaging.VehicleConfigMsgPayload()
servicerConfigOutData.massSC = scObjectServicer.hub.mHub
servicerVehicleConfigMsg = messaging.VehicleConfigMsg().write(servicerConfigOutData)
debrisConfigOutData = messaging.VehicleConfigMsgPayload()
debrisConfigOutData.massSC = scObjectDebris.hub.mHub
debrisVehicleConfigMsg = messaging.VehicleConfigMsg().write(debrisConfigOutData)
# clear prior gravitational body and SPICE setup definitions
gravFactory = simIncludeGravBody.gravBodyFactory()
# setup Earth Gravity Body
earth = gravFactory.createEarth()
earth.isCentralBody = True # ensure this is the central gravitational body
mu = earth.mu
# attach gravity model to spaceCraftPlus
gravFactory.addBodiesTo(scObjectServicer)
gravFactory.addBodiesTo(scObjectDebris)
# setup MSM module
MSMmodule = msmForceTorque.MsmForceTorque()
MSMmodule.ModelTag = "msmForceTorqueTag"
scSim.AddModelToTask(dynTaskName, MSMmodule)
# define electric potentials
voltServicerInMsgData = messaging.VoltMsgPayload()
voltServicerInMsgData.voltage = 25000. # [V] servicer potential
voltServicerInMsg = messaging.VoltMsg().write(voltServicerInMsgData)
voltDebrisInMsgData = messaging.VoltMsgPayload()
voltDebrisInMsgData.voltage = -25000. # [V] debris potential
voltDebrisInMsg = messaging.VoltMsg().write(voltDebrisInMsgData)
# create a list of sphere body-fixed locations and associated radii
spPosListServicer = [[0., 0., 0.]] # one sphere located at origin of body frame
rListServicer = [2.] # radius of sphere is 2m
spPosListDebris = [[0., 0., 0.]] # one sphere located at origin of body frame
rListDebris = [3.] # radius of sphere is 3m
# add spacecraft to state
MSMmodule.addSpacecraftToModel(scObjectServicer.scStateOutMsg, messaging.DoubleVector(rListServicer),
unitTestSupport.npList2EigenXdVector(spPosListServicer))
MSMmodule.addSpacecraftToModel(scObjectDebris.scStateOutMsg, messaging.DoubleVector(rListDebris),
unitTestSupport.npList2EigenXdVector(spPosListDebris))
# subscribe input messages to module
MSMmodule.voltInMsgs[0].subscribeTo(voltServicerInMsg)
MSMmodule.voltInMsgs[1].subscribeTo(voltDebrisInMsg)
# setup extForceTorque module for Servicer
# the electrostatic force from the MSM module is read in through the messaging system
extFTObjectServicer = extForceTorque.ExtForceTorque()
extFTObjectServicer.ModelTag = "eForceServicer"
extFTObjectServicer.cmdForceInertialInMsg.subscribeTo(MSMmodule.eForceOutMsgs[0])
scObjectServicer.addDynamicEffector(extFTObjectServicer)
scSim.AddModelToTask(dynTaskName, extFTObjectServicer)
# setup extForceTorque module for Debris
# the electrostatic force from the MSM module is read in through the messaging system
extFTObjectDebris = extForceTorque.ExtForceTorque()
extFTObjectDebris.ModelTag = "eForceDebris"
extFTObjectDebris.cmdForceInertialInMsg.subscribeTo(MSMmodule.eForceOutMsgs[1])
scObjectDebris.addDynamicEffector(extFTObjectDebris)
scSim.AddModelToTask(dynTaskName, extFTObjectDebris)
# setup extForceTorque module
# the control force from the ET relative motion control module is read in through the messaging system
extFTObjectServicerControl = extForceTorque.ExtForceTorque()
extFTObjectServicerControl.ModelTag = "controlServicer"
scObjectServicer.addDynamicEffector(extFTObjectServicerControl)
scSim.AddModelToTask(dynTaskName, extFTObjectServicerControl)
# add the simple Navigation sensor module. This sets the SC attitude, rate, position
# velocity navigation message
sNavObjectServicer = simpleNav.SimpleNav()
sNavObjectServicer.ModelTag = "SimpleNavigation"
sNavObjectServicer.scStateInMsg.subscribeTo(scObjectServicer.scStateOutMsg)
scSim.AddModelToTask(dynTaskName, sNavObjectServicer)
sNavObjectDebris = simpleNav.SimpleNav()
sNavObjectDebris.ModelTag = "SimpleNavigation3"
sNavObjectDebris.scStateInMsg.subscribeTo(scObjectDebris.scStateOutMsg)
scSim.AddModelToTask(dynTaskName, sNavObjectDebris)
# ----- fsw ----- #
fswProcessName = "fswProcess"
fswTaskName = "fswTask"
fswProcess = scSim.CreateNewProcess(fswProcessName, 1)
fswTimeStep = macros.sec2nano(300.0)
fswProcess.addTask(scSim.CreateNewTask(fswTaskName, fswTimeStep))
# setup ET Relative Motion Control module
etSphericalControlObj = etSphericalControl.etSphericalControl()
etSphericalControlObj.ModelTag = "ETcontrol"
# connect required messages
etSphericalControlObj.servicerTransInMsg.subscribeTo(sNavObjectServicer.transOutMsg) # servicer translation
etSphericalControlObj.debrisTransInMsg.subscribeTo(sNavObjectDebris.transOutMsg) # debris translation
etSphericalControlObj.servicerAttInMsg.subscribeTo(sNavObjectServicer.attOutMsg) # servicer attitude
etSphericalControlObj.servicerVehicleConfigInMsg.subscribeTo(servicerVehicleConfigMsg) # servicer mass
etSphericalControlObj.debrisVehicleConfigInMsg.subscribeTo(debrisVehicleConfigMsg) # debris mass
etSphericalControlObj.eForceInMsg.subscribeTo(MSMmodule.eForceOutMsgs[0]) # eForce on servicer (for feed-forward)
# set module parameters
# feedback gain matrices
Ki = 4e-7
Pi = 1.85 * Ki ** 0.5
etSphericalControlObj.K = [Ki, 0.0, 0.0,
0.0, Ki, 0.0,
0.0, 0.0, Ki]
etSphericalControlObj.P = [Pi, 0.0, 0.0,
0.0, Pi, 0.0,
0.0, 0.0, Pi]
# desired relative position in spherical coordinates (reference state)
etSphericalControlObj.L_r = 30.0 # separation distance
etSphericalControlObj.theta_r = 0. # in-plane rotation angle
etSphericalControlObj.phi_r = 0. # out-of-plane rotation angle
etSphericalControlObj.mu = mu # gravitational parameter
# add module to fsw task
scSim.AddModelToTask(fswTaskName, etSphericalControlObj)
# connect output control thrust force with external force on servicer
extFTObjectServicerControl.cmdForceInertialInMsg.subscribeTo(etSphericalControlObj.forceInertialOutMsg)
#
# set initial Spacecraft States
#
# setup the servicer orbit using classical orbit elements
oe = orbitalMotion.ClassicElements()
oe.a = 42164. * 1e3 # [m] geostationary orbit
oe.e = 0.
oe.i = 0.
oe.Omega = 0.
oe.omega = 0
oe.f = 0.
r_SN, v_SN = orbitalMotion.elem2rv(mu, oe)
scObjectServicer.hub.r_CN_NInit = r_SN # m
scObjectServicer.hub.v_CN_NInit = v_SN # m/s
oe = orbitalMotion.rv2elem(mu, r_SN, v_SN)
# setup debris object states
r_DS = np.array([0, -50.0, 0.0]) # relative position of debris, 50m behind servicer in along-track direction
r_DN = r_DS + r_SN
v_DN = v_SN
scObjectDebris.hub.r_CN_NInit = r_DN # m
scObjectDebris.hub.v_CN_NInit = v_DN # m/s
n = np.sqrt(mu / oe.a / oe.a / oe.a)
P = 2. * np.pi / n # orbit period
#
# Setup data logging before the simulation is initialized
#
numDataPoints = 1000
simulationTime = macros.sec2nano(1. * P)
samplingTime = simulationTime // (numDataPoints - 1)
dataRecS = scObjectServicer.scStateOutMsg.recorder(samplingTime)
dataRecD = scObjectDebris.scStateOutMsg.recorder(samplingTime)
scSim.AddModelToTask(dynTaskName, dataRecS)
scSim.AddModelToTask(dynTaskName, dataRecD)
# if this scenario is to interface with the BSK Viz, uncomment the following lines
# to save the BSK data to a file, uncomment the saveFile line below
if vizSupport.vizFound:
# setup MSM information
msmInfoServicer = vizInterface.MultiSphereInfo()
msmInfoServicer.msmChargeInMsg.subscribeTo(MSMmodule.chargeMsmOutMsgs[0])
msmServicerList = []
for (pos, rad) in zip(spPosListServicer, rListServicer):
msmServicer = vizInterface.MultiSphere()
msmServicer.position = pos
msmServicer.radius = rad
msmServicer.isOn = 1
msmServicer.maxValue = 30e-6 # Coulomb
msmServicer.currentValue = 4e-6 # Coulomb
msmServicerList.append(msmServicer)
msmInfoServicer.msmList = vizInterface.MultiSphereVector(msmServicerList)
msmInfoDebris = vizInterface.MultiSphereInfo()
msmInfoDebris.msmChargeInMsg.subscribeTo(MSMmodule.chargeMsmOutMsgs[1])
msmDebrisList = []
for (pos, rad) in zip(spPosListDebris, rListDebris):
msmDebris = vizInterface.MultiSphere()
msmDebris.position = pos
msmDebris.radius = rad
msmDebris.isOn = 1
msmDebris.maxValue = 30e-6 # Coulomb
msmDebris.currentValue = 4e-6 # Coulomb
msmDebris.neutralOpacity = 50 # opacity value between 0 and 255
msmDebrisList.append(msmDebris)
msmInfoDebris.msmList = vizInterface.MultiSphereVector(msmDebrisList)
viz = vizSupport.enableUnityVisualization(scSim, dynTaskName, [scObjectServicer, scObjectDebris]
# , saveFile=fileName
, msmInfoList=[msmInfoServicer, msmInfoDebris]
)
#
# initialize Simulation
#
scSim.InitializeSimulation()
#
# configure a simulation stop time and execute the simulation run
#
scSim.ConfigureStopTime(simulationTime)
scSim.ExecuteSimulation()
# retrieve the logged data
posDataS = dataRecS.r_BN_N
velDataS = dataRecS.v_BN_N
posDataD = dataRecD.r_BN_N
velDataD = dataRecD.v_BN_N
timeData = dataRecS.times()
np.set_printoptions(precision=16)
figureList = plotOrbits(timeData, posDataS, velDataS, posDataD, velDataD, oe, mu, P, earth)
if show_plots:
plt.show()
# close the plots being saved off to avoid over-writing old and new figures
plt.close("all")
return figureList
def plotOrbits(timeData, posDataS, velDataS, posDataD, velDataD, oe, mu, P, planet):
# draw the inertial position vector components
plt.close("all") # clears out plots from earlier test runs
plt.figure(1)
fig = plt.gcf()
ax = fig.gca()
ax.ticklabel_format(useOffset=False, style='plain')
relPosData = posDataS[:, 0:3] - posDataD[:, 0:3]
relPosMagn = np.linalg.norm(relPosData, axis=1)
plt.plot(timeData * macros.NANO2SEC / P, relPosMagn[:])
plt.xlabel('Time [orbits]')
plt.ylabel('Separation [m]')
figureList = {}
pltName = fileName + "1"
figureList[pltName] = plt.figure(1)
# draw orbit in perifocal frame
p = oe.a * (1 - oe.e * oe.e)
plt.figure(2)
plt.axis('equal')
# draw the planet
fig = plt.gcf()
ax = fig.gca()
planetColor = '#008800'
planetRadius = planet.radEquator / 1000
ax.add_artist(plt.Circle((0, 0), planetRadius, color=planetColor))
# draw the actual orbit
rData = []
fData = []
for idx in range(0, len(posDataS)):
oeData = orbitalMotion.rv2elem(mu, posDataS[idx, 0:3], velDataS[idx, 0:3])
rData.append(oeData.rmag)
fData.append(oeData.f + oeData.omega - oe.omega)
plt.plot(rData * np.cos(fData) / 1000, rData * np.sin(fData) / 1000, color='#aa0000', linewidth=3.0)
# draw the full osculating orbit from the initial conditions
fData = np.linspace(0, 2 * np.pi, 100)
rData = []
for idx in range(0, len(fData)):
rData.append(p / (1 + oe.e * np.cos(fData[idx])))
plt.plot(rData * np.cos(fData) / 1000, rData * np.sin(fData) / 1000, '--', color='#555555'
)
plt.xlabel('$x$ Cord. [km]')
plt.ylabel('$y$ Cord. [km]')
plt.grid()
pltName = fileName + "2"
figureList[pltName] = plt.figure(2)
plt.figure(3)
fig = plt.gcf()
ax = fig.gca()
ax.ticklabel_format(useOffset=False, style='plain')
aData = []
for idx in range(0, len(posDataS)):
oeData = orbitalMotion.rv2elem(mu, posDataD[idx, 0:3], velDataD[idx, 0:3])
aData.append(oeData.a)
plt.plot(timeData * macros.NANO2SEC / P, (aData - oe.a) / 1000., color='#aa0000', linewidth=3.0)
plt.xlabel('Time [orbits]')
plt.ylabel('Increase of semi-major axis [km]')
pltName = fileName + "3"
figureList[pltName] = plt.figure(3)
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
)