#
# ISC License
#
# Copyright (c) 2023, 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 the capabilities of :ref:`spinningBodyTwoDOFStateEffector`, which represents an effector with
two spin axis. The two-degree-of-freedom formulation allows the simulation of one rigid body attached to the hub through
a universal joint, or a chain of two rigid bodies each attached through a single hinge. The spin axis and mass
distribution of the rigid bodies are arbitrary.
The scenario can be run with either one or two rigid bodies. The one-panel formulation consists of a cylindrical flat
panel that rotates about two axis in a universal-joint configuration. The panel has similar dimensions to the hub. The
two-panel formulation uses two flat cuboid panels that rotate about perpendicular hinges. The first panel connects
directly to the hub, whereas the second connects to the first.
The script is found in the folder ``basilisk/examples`` and executed by using::
python3 scenarioSpinningBodiesTwoDOF.py
The scenario outputs two plots: one for the time history of both angles, and another for the time history of both angle
rates. the scenario also creates a comprehensive Vizard simulation which creates appropriate to-scale models for each
simulation type.
Illustration of Simulation Results
----------------------------------
::
show_plots = True, numberPanels = 1
Here, only a single panel is simulated. The panel connects to the hub through a universal, dual-axis joint. The time
history for both angles and angle rates is shown below. Note how decoupled the two angles are. This is because the
module is simulating a universal joint, so each axis behaves independently from each other. The impact of the motion of
the panel is also shown in the velocity and angular velocity plots. The initial transient in the time history of these
two quantities matches the motion of the panel.
.. image:: /_images/Scenarios/scenarioSpinningBodiesTwoDOFtheta1.svg
:align: center
.. image:: /_images/Scenarios/scenarioSpinningBodiesTwoDOFthetaDot1.svg
:align: center
.. image:: /_images/Scenarios/scenarioSpinningBodiesTwoDOFvelocity1.svg
:align: center
.. image:: /_images/Scenarios/scenarioSpinningBodiesTwoDOFangularVelocity1.svg
:align: center
::
show_plots = True, numberPanels = 2
In this case, two panels are simulated. Each rotates about a one-degree-of-freedom hinge. The spin axis are perpendicular
to each other to show how any spin axis can be chosen. Note that in this case, there is much more significant coupling
between the two angles, with them being in-phase with each other. As before, the transient motion of the velocity and
angular velocity states match the motion of the panels.
.. image:: /_images/Scenarios/scenarioSpinningBodiesTwoDOFtheta2.svg
:align: center
.. image:: /_images/Scenarios/scenarioSpinningBodiesTwoDOFthetaDot2.svg
:align: center
.. image:: /_images/Scenarios/scenarioSpinningBodiesTwoDOFvelocity2.svg
:align: center
.. image:: /_images/Scenarios/scenarioSpinningBodiesTwoDOFangularVelocity2.svg
:align: center
"""
#
# Basilisk Scenario Script and Integrated Test
#
# Purpose: Illustrates the different simulation capabilities of the 2DOF modules
# Author: João Vaz Carneiro
# Creation Date: Feb 22, 2023
#
import os
import matplotlib.pyplot as plt
import numpy as np
from Basilisk.utilities import SimulationBaseClass, vizSupport, simIncludeGravBody
from Basilisk.simulation import spacecraft, spinningBodyTwoDOFStateEffector
from Basilisk.utilities import macros, orbitalMotion, unitTestSupport
from Basilisk import __path__
bskPath = __path__[0]
fileName = os.path.basename(os.path.splitext(__file__)[0])
[docs]
def run(show_plots, numberPanels):
"""
The scenarios can be run with the followings setups parameters:
Args:
show_plots (bool): Determines if the script should display plots
numberPanels (int): Choose how many panels to simulate (1 or 2)
"""
# 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 integration update time
simulationTimeStep = macros.sec2nano(.01)
dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep))
#
# Set up the simulation tasks/objects
#
# Define the spacecraft's properties
massSC = 400
diameter = 2
height = 4
# Initialize spacecraft object and set properties
scObject = spacecraft.Spacecraft()
scObject.ModelTag = "scObject"
scObject.hub.mHub = massSC
scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]]
scObject.hub.IHubPntBc_B = [[massSC / 16 * diameter ** 2 + massSC / 12 * height ** 2, 0.0, 0.0],
[0.0, massSC / 16 * diameter ** 2 + massSC / 12 * height ** 2, 0.0],
[0.0, 0.0, massSC / 8 * diameter ** 2]]
# Set the spacecraft's initial conditions
scObject.hub.r_CN_NInit = np.array([0, 0, 0]) # m - r_CN_N
scObject.hub.v_CN_NInit = np.array([0, 0, 0]) # m/s - v_CN_N
scObject.hub.sigma_BNInit = [[0.0], [0.0], [0.0]]
scObject.hub.omega_BN_BInit = [[0.05], [-0.05], [0.05]]
# Create two hinged rigid bodies
spinningBody = spinningBodyTwoDOFStateEffector.SpinningBodyTwoDOFStateEffector()
spinningBody.ModelTag = "SpinningBody"
# Set up the spinning bodies module
if numberPanels == 1:
# Define the properties of the panel (cylinder)
mass = 50
radius = diameter
thickness = 0.1
# Define the module's properties from the panels
spinningBody.mass1 = 0.0
spinningBody.mass2 = mass
spinningBody.IS1PntSc1_S1 = [[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0]]
spinningBody.IS2PntSc2_S2 = [[mass / 12 * (3 * radius ** 2 + thickness ** 2), 0.0, 0.0],
[0.0, mass / 12 * (3 * radius ** 2 + thickness ** 2), 0.0],
[0.0, 0.0, mass / 2 * radius ** 2]]
spinningBody.dcm_S10B = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
spinningBody.dcm_S20S1 = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
spinningBody.r_Sc1S1_S1 = [[0.0], [0.0], [0.0]]
spinningBody.r_Sc2S2_S2 = [[0.0], [radius], [0.0]]
spinningBody.r_S1B_B = [[0.0], [diameter / 2], [height / 2 - thickness / 2]]
spinningBody.r_S2S1_S1 = [[0.0], [0.0], [0.0]]
spinningBody.s1Hat_S1 = [[1], [0], [0]]
spinningBody.s2Hat_S2 = [[0], [1], [0]]
spinningBody.k1 = 100.0
spinningBody.k2 = 100.0
spinningBody.c1 = 50.0
spinningBody.c2 = 50.0
spinningBody.theta1Init = 30 * macros.D2R
spinningBody.theta2Init = 30 * macros.D2R
spinningBody.theta1DotInit = 0.0 * macros.D2R
spinningBody.theta2DotInit = 0.0 * macros.D2R
elif numberPanels == 2:
# Define the properties of the panels (rectangular cuboids)
mass = 20
length = 2 * diameter
width = diameter
thickness = 0.1
# Define the module's properties from the panels
spinningBody.mass1 = mass
spinningBody.mass2 = mass
spinningBody.IS1PntSc1_S1 = [[mass / 12 * (length ** 2 + thickness ** 2), 0.0, 0.0],
[0.0, mass / 12 * (thickness ** 2 + width ** 2), 0.0],
[0.0, 0.0, mass / 12 * (length ** 2 + width ** 2)]]
spinningBody.IS2PntSc2_S2 = [[mass / 12 * (length ** 2 + thickness ** 2), 0.0, 0.0],
[0.0, mass / 12 * (thickness ** 2 + width ** 2), 0.0],
[0.0, 0.0, mass / 12 * (length ** 2 + width ** 2)]]
spinningBody.dcm_S10B = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
spinningBody.dcm_S20S1 = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
spinningBody.r_Sc1S1_S1 = [[0.0], [length / 2], [0.0]]
spinningBody.r_Sc2S2_S2 = [[-width / 2], [0.0], [0.0]]
spinningBody.r_S1B_B = [[0.0], [diameter / 2], [height / 2 - thickness / 2]]
spinningBody.r_S2S1_S1 = [[- width / 2], [length / 2], [0.0]]
spinningBody.s1Hat_S1 = [[1], [0], [0]]
spinningBody.s2Hat_S2 = [[0], [1], [0]]
spinningBody.k1 = 50.0
spinningBody.k2 = 50.0
spinningBody.c1 = 30.0
spinningBody.c2 = 30.0
spinningBody.theta1Init = 60.0 * macros.D2R
spinningBody.theta2Init = 60.0 * macros.D2R
spinningBody.theta1DotInit = 0.0 * macros.D2R
spinningBody.theta2DotInit = 0.0 * macros.D2R
else:
print(f"Cannot simulate {numberPanels} panels.")
return
# Add spinning body to spacecraft
scObject.addStateEffector(spinningBody)
# Add modules to simulation process
scSim.AddModelToTask(simTaskName, scObject)
scSim.AddModelToTask(simTaskName, spinningBody)
# Set up the message recorders and add them to the task
datLog = scObject.scStateOutMsg.recorder()
theta1Data = spinningBody.spinningBodyOutMsgs[0].recorder()
theta2Data = spinningBody.spinningBodyOutMsgs[1].recorder()
scStateData = scObject.scStateOutMsg.recorder()
scSim.AddModelToTask(simTaskName, datLog)
scSim.AddModelToTask(simTaskName, theta1Data)
scSim.AddModelToTask(simTaskName, theta2Data)
scSim.AddModelToTask(simTaskName, scStateData)
#
# Set up Vizard visualization
#
scBodyList = [scObject]
if numberPanels == 1:
scBodyList.append(["panel", spinningBody.spinningBodyConfigLogOutMsgs[1]])
else:
scBodyList.append(["panel1", spinningBody.spinningBodyConfigLogOutMsgs[0]])
scBodyList.append(["panel2", spinningBody.spinningBodyConfigLogOutMsgs[1]])
if vizSupport.vizFound:
viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scBodyList
# , saveFile=fileName + str(numberPanels)
)
vizSupport.createCustomModel(viz
, simBodiesToModify=[scObject.ModelTag]
, modelPath="CYLINDER"
, scale=[diameter, diameter, height / 2]
, color=vizSupport.toRGBA255("blue"))
if numberPanels == 1:
vizSupport.createCustomModel(viz
, simBodiesToModify=["panel"]
, modelPath="CYLINDER"
, scale=[2 * radius, 2 * radius, thickness]
, color=vizSupport.toRGBA255("green"))
elif numberPanels == 2:
vizSupport.createCustomModel(viz
, simBodiesToModify=["panel1"]
, modelPath="CUBE"
, scale=[width, length, thickness]
, color=vizSupport.toRGBA255("green"))
vizSupport.createCustomModel(viz
, simBodiesToModify=["panel2"]
, modelPath="CUBE"
, scale=[width, length, thickness]
, color=vizSupport.toRGBA255("green"))
viz.settings.orbitLinesOn = -1
# Initialize the simulation
scSim.InitializeSimulation()
# Configure a simulation stop time and execute the simulation run
simulationTime = macros.min2nano(1)
scSim.ConfigureStopTime(simulationTime)
scSim.ExecuteSimulation()
scSim.ShowExecutionOrder()
# Retrieve the logged data
theta1 = theta1Data.theta
theta1Dot = theta1Data.thetaDot
theta2 = theta2Data.theta
theta2Dot = theta2Data.thetaDot
v_BN_N = scStateData.v_BN_N
omega_BN_B = scStateData.omega_BN_B
#
# plot the results
#
figureList = {}
plt.close("all") # clears out plots from earlier test runs
plt.figure(1)
plt.clf()
plt.plot(theta1Data.times() * macros.NANO2SEC, macros.R2D * theta1, label=r'$\theta_1$')
plt.plot(theta2Data.times() * macros.NANO2SEC, macros.R2D * theta2, label=r'$\theta_2$')
plt.legend()
plt.xlabel('time [s]')
plt.ylabel(r'$\theta$ [deg]')
pltName = fileName + "theta" + str(int(numberPanels))
figureList[pltName] = plt.figure(1)
plt.figure(2)
plt.clf()
plt.plot(theta1Data.times() * macros.NANO2SEC, macros.R2D * theta1Dot, label=r'$\dot{\theta}_1$')
plt.plot(theta1Data.times() * macros.NANO2SEC, macros.R2D * theta2Dot, label=r'$\dot{\theta}_2$')
plt.legend()
plt.xlabel('time [s]')
plt.ylabel(r'$\dot{\theta}$ [deg/s]')
pltName = fileName + "thetaDot" + str(int(numberPanels))
figureList[pltName] = plt.figure(2)
plt.figure(3)
plt.clf()
for idx in range(3):
plt.plot(theta1Data.times() * macros.NANO2SEC, v_BN_N[:, idx],
color=unitTestSupport.getLineColor(idx, 3),
label='$v_{BN,' + str(idx) + '}$')
plt.legend()
plt.xlabel('time [s]')
plt.ylabel(r'Velocity [m/s]')
pltName = fileName + "velocity" + str(int(numberPanels))
figureList[pltName] = plt.figure(3)
plt.figure(4)
plt.clf()
for idx in range(3):
plt.plot(theta1Data.times() * macros.NANO2SEC, omega_BN_B[:, idx],
color=unitTestSupport.getLineColor(idx, 3),
label=r'$\omega_{BN,' + str(idx) + '}$')
plt.legend()
plt.xlabel('time [s]')
plt.ylabel(r'Angular Velocity [rad/s]')
pltName = fileName + "angularVelocity" + str(int(numberPanels))
figureList[pltName] = plt.figure(4)
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
2, # numberPanels
)